Glonass.java

  1. /* Copyright 2002-2025 CS GROUP
  2.  * Licensed to CS GROUP (CS) under one or more
  3.  * contributor license agreements.  See the NOTICE file distributed with
  4.  * this work for additional information regarding copyright ownership.
  5.  * CS licenses this file to You under the Apache License, Version 2.0
  6.  * (the "License"); you may not use this file except in compliance with
  7.  * the License.  You may obtain a copy of the License at
  8.  *
  9.  *   http://www.apache.org/licenses/LICENSE-2.0
  10.  *
  11.  * Unless required by applicable law or agreed to in writing, software
  12.  * distributed under the License is distributed on an "AS IS" BASIS,
  13.  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14.  * See the License for the specific language governing permissions and
  15.  * limitations under the License.
  16.  */
  17. package org.orekit.gnss.attitude;

  19. import org.hipparchus.Field;
  20. import org.hipparchus.CalculusFieldElement;
  21. import org.hipparchus.analysis.CalculusFieldUnivariateFunction;
  22. import org.hipparchus.analysis.UnivariateFunction;
  23. import org.hipparchus.analysis.solvers.AllowedSolution;
  24. import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
  25. import org.hipparchus.analysis.solvers.FieldBracketingNthOrderBrentSolver;
  26. import org.hipparchus.analysis.solvers.UnivariateSolverUtils;
  27. import org.hipparchus.util.FastMath;
  28. import org.orekit.frames.Frame;
  29. import org.orekit.time.AbsoluteDate;
  30. import org.orekit.utils.ExtendedPositionProvider;
  31. import org.orekit.utils.TimeStampedAngularCoordinates;
  32. import org.orekit.utils.TimeStampedFieldAngularCoordinates;

  34. /**
  35.  * Attitude providers for Glonass navigation satellites.
  36.  * <p>
  37.  * This class is based on the May 2017 version of J. Kouba eclips.f
  38.  * subroutine available at <a href="http://acc.igs.org/orbits">IGS Analysis
  39.  * Center Coordinator site</a>. The eclips.f code itself is not used ; its
  40.  * hard-coded data are used and its low level models are used, but the
  41.  * structure of the code and the API have been completely rewritten.
  42.  * </p>
  43.  * @author J. Kouba original fortran routine
  44.  * @author Luc Maisonobe Java translation
  45.  * @since 9.2
  46.  */
  47. public class Glonass extends AbstractGNSSAttitudeProvider {

  49.     /** Default yaw rates for all spacecrafts in radians per seconds. */
  50.     public static final double DEFAULT_YAW_RATE = FastMath.toRadians(0.250);

  52.     /** Satellite-Sun angle limit for a midnight turn maneuver. */
  53.     private static final double NIGHT_TURN_LIMIT = FastMath.toRadians(180.0 - 14.20);

  55.     /** Initial yaw end at iterative search start. */
  56.     private static final double YAW_END_ZERO = FastMath.toRadians(75.0);

  58.     /** No margin on turn end for Glonass. */
  59.     private static final double END_MARGIN = 0.0;

  61.     /** Yaw rate. */
  62.     private final double yawRate;

  64.     /** Simple constructor.
  65.      * @param yawRate yaw rate to use in radians per seconds (typically {@link #DEFAULT_YAW_RATE})
  66.      * @param validityStart start of validity for this provider
  67.      * @param validityEnd end of validity for this provider
  68.      * @param sun provider for Sun position
  69.      * @param inertialFrame inertial frame where velocity are computed
  70.      */
  71.     public Glonass(final double yawRate,
  72.                    final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
  73.                    final ExtendedPositionProvider sun, final Frame inertialFrame) {
  74.         super(validityStart, validityEnd, sun, inertialFrame);
  75.         this.yawRate = yawRate;
  76.     }

  78.     /** {@inheritDoc} */
  79.     @Override
  80.     protected TimeStampedAngularCoordinates correctedYaw(final GNSSAttitudeContext context) {

  82.         // noon beta angle limit from yaw rate
  83.         final double realBeta = context.beta(context.getDate());
  84.         final double muRate   = context.getMuRate();
  85.         final double aNight   = NIGHT_TURN_LIMIT;
  86.         double       aNoon    = FastMath.atan(muRate / yawRate);
  87.         if (FastMath.abs(realBeta) < aNoon) {
  88.             final UnivariateFunction f = yawEnd -> {
  89.                 final double delta =  muRate * yawEnd / yawRate;
  90.                 return yawEnd - 0.5 * FastMath.abs(context.computePhi(realBeta,  delta) -
  91.                                                    context.computePhi(realBeta, -delta));
  92.             };
  93.             final double[] bracket = UnivariateSolverUtils.bracket(f, YAW_END_ZERO, 0.0, FastMath.PI);
  94.             final double yawEnd = new BracketingNthOrderBrentSolver(1.0e-14, 1.0e-8, 1.0e-15, 5).
  95.                                   solve(50, f, bracket[0], bracket[1], AllowedSolution.ANY_SIDE);
  96.             aNoon = muRate * yawEnd / yawRate;
  97.         }

  99.         final double cNoon  = FastMath.cos(aNoon);
  100.         final double cNight = FastMath.cos(aNight);

  102.         if (context.setUpTurnRegion(cNight, cNoon)) {

  104.             context.setHalfSpan(context.inSunSide() ?
  105.                                 aNoon :
  106.                                 context.inOrbitPlaneAbsoluteAngle(aNight - FastMath.PI),
  107.                                 END_MARGIN);
  108.             if (context.inTurnTimeRange()) {

  110.                 // we need to ensure beta sign does not change during the turn
  111.                 final double beta     = context.getSecuredBeta();
  112.                 final double phiStart = context.getYawStart(beta);
  113.                 final double dtStart  = context.timeSinceTurnStart();

  115.                 final double phiDot;
  116.                 final double linearPhi;
  117.                 final double phiEnd    = context.getYawEnd(beta);
  118.                 if (context.inSunSide()) {
  119.                     // noon turn
  120.                     phiDot    = -FastMath.copySign(yawRate, beta);
  121.                     linearPhi = phiStart + phiDot * dtStart;
  122.                 } else {
  123.                     // midnight turn
  124.                     phiDot    = FastMath.copySign(yawRate, beta);
  125.                     linearPhi = phiStart + phiDot * dtStart;

  127.                     if (phiEnd / linearPhi < 0 || phiEnd / linearPhi > 1) {
  128.                         // this turn limitation is only computed for midnight turns in Kouba model
  129.                         // we don't understand yet why it doesn't apply to noon turns
  130.                         return context.turnCorrectedAttitude(phiEnd, 0.0);
  131.                     }

  133.                 }

  135.                 return context.turnCorrectedAttitude(linearPhi, phiDot);

  137.             }

  139.         }

  141.         // in nominal yaw mode
  142.         return context.nominalYaw(context.getDate());

  144.     }

  146.     /** {@inheritDoc} */
  147.     @Override
  148.     protected <T extends CalculusFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {

  150.         final Field<T> field = context.getDate().getField();

  152.         // noon beta angle limit from yaw rate
  153.         final T realBeta = context.beta(context.getDate());
  154.         final T muRate   = context.getMuRate();
  155.         final T aNight   = field.getZero().newInstance(NIGHT_TURN_LIMIT);
  156.         T       aNoon    = FastMath.atan(muRate.divide(yawRate));
  157.         if (FastMath.abs(realBeta).getReal() < aNoon.getReal()) {
  158.             final CalculusFieldUnivariateFunction<T> f = yawEnd -> {
  159.                 final T delta = muRate.multiply(yawEnd).divide(yawRate);
  160.                 return yawEnd.subtract(FastMath.abs(context.computePhi(realBeta, delta).
  161.                                                     subtract(context.computePhi(realBeta, delta.negate()))).
  162.                                        multiply(0.5));
  163.             };
  164.             final T[] bracket = UnivariateSolverUtils.bracket(f, field.getZero().newInstance(YAW_END_ZERO),
  165.                                                               field.getZero(), field.getZero().getPi());
  166.             final T yawEnd = new FieldBracketingNthOrderBrentSolver<>(field.getZero().newInstance(1.0e-14),
  167.                                                                       field.getZero().newInstance(1.0e-8),
  168.                                                                       field.getZero().newInstance(1.0e-15),
  169.                                                                       5).
  170.                             solve(50, f, bracket[0], bracket[1], AllowedSolution.ANY_SIDE);
  171.             aNoon = muRate.multiply(yawEnd).divide(yawRate);
  172.         }

  174.         final double cNoon  = FastMath.cos(aNoon.getReal());
  175.         final double cNight = FastMath.cos(aNight.getReal());

  177.         if (context.setUpTurnRegion(cNight, cNoon)) {

  179.             context.setHalfSpan(context.inSunSide() ?
  180.                                 aNoon :
  181.                                 context.inOrbitPlaneAbsoluteAngle(aNight.subtract(aNight.getPi())),
  182.                                 END_MARGIN);
  183.             if (context.inTurnTimeRange()) {

  185.                 // we need to ensure beta sign does not change during the turn
  186.                 final T beta     = context.getSecuredBeta();
  187.                 final T phiStart = context.getYawStart(beta);
  188.                 final T dtStart  = context.timeSinceTurnStart();

  190.                 final T phiDot;
  191.                 final T linearPhi;
  192.                 final T phiEnd    = context.getYawEnd(beta);
  193.                 if (context.inSunSide()) {
  194.                     // noon turn
  195.                     phiDot    = field.getZero().newInstance(-FastMath.copySign(yawRate, beta.getReal()));
  196.                     linearPhi = phiStart.add(phiDot.multiply(dtStart));
  197.                 } else {
  198.                     // midnight turn
  199.                     phiDot    = field.getZero().newInstance(FastMath.copySign(yawRate, beta.getReal()));
  200.                     linearPhi = phiStart.add(phiDot.multiply(dtStart));

  202.                     // this turn limitation is only computed for midnight turns in Kouba model
  203.                     // we don't understand yet why it doesn't apply to noon turns
  204.                     if (phiEnd.getReal() / linearPhi.getReal() < 0 || phiEnd.getReal() / linearPhi.getReal() > 1) {
  205.                         return context.turnCorrectedAttitude(phiEnd, field.getZero());
  206.                     }

  208.                 }

  210.                 return context.turnCorrectedAttitude(linearPhi, phiDot);


  213.             }

  215.         }

  217.         // in nominal yaw mode
  218.         return context.nominalYaw(context.getDate());

  220.     }

  222. }
Created with JaCoCo 0.8.12.202403310830