GPSBlockIIA.java

  1. /* Copyright 2002-2018 CS Systèmes d'Information
  2.  * Licensed to CS Systèmes d'Information (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.RealFieldElement;
  21. import org.hipparchus.util.FastMath;
  22. import org.orekit.frames.Frame;
  23. import org.orekit.time.AbsoluteDate;
  24. import org.orekit.utils.ExtendedPVCoordinatesProvider;
  25. import org.orekit.utils.TimeStampedAngularCoordinates;
  26. import org.orekit.utils.TimeStampedFieldAngularCoordinates;

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

  46.     /** Serializable UID. */
  47.     private static final long serialVersionUID = 20171114L;

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

  52.     /** Bias. */
  53.     private static final double YAW_BIAS = FastMath.toRadians(0.5);

  55.     /** Yaw rates for all spacecrafts. */
  56.     private static final double[] YAW_RATES = new double[] {
  57.         0.1211, 0.1339, 0.1230, 0.1233, 0.1180, 0.1266, 0.1269, 0.1033,
  58.         0.1278, 0.0978, 0.2000, 0.1990, 0.2000, 0.0815, 0.1303, 0.0838,
  59.         0.1401, 0.1069, 0.0980, 0.1030, 0.1366, 0.1025, 0.1140, 0.1089,
  60.         0.1001, 0.1227, 0.1194, 0.1260, 0.1228, 0.1165, 0.0969, 0.1140
  61.     };

  63.     /** Margin on turn end. */
  64.     private final double END_MARGIN = 1800.0;

  66.     /** Yaw rate for current spacecraft. */
  67.     private final double yawRate;

  69.     /** Simple constructor.
  70.      * @param validityStart start of validity for this provider
  71.      * @param validityEnd end of validity for this provider
  72.      * @param sun provider for Sun position
  73.      * @param inertialFrame inertial frame where velocity are computed
  74.      * @param prnNumber number within the GPS constellation (between 1 and 32)
  75.      */
  76.     public GPSBlockIIA(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
  77.                        final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame, final int prnNumber) {
  78.         super(validityStart, validityEnd, sun, inertialFrame);
  79.         yawRate = FastMath.toRadians(YAW_RATES[prnNumber - 1]);
  80.     }

  82.     /** {@inheritDoc} */
  83.     @Override
  84.     protected TimeStampedAngularCoordinates correctedYaw(final GNSSAttitudeContext context) {

  86.         // noon beta angle limit from yaw rate
  87.         final double aNoon  = FastMath.atan(context.getMuRate() / yawRate);
  88.         final double aNight = NIGHT_TURN_LIMIT;
  89.         final double cNoon  = FastMath.cos(aNoon);
  90.         final double cNight = FastMath.cos(aNight);

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

  94.             final double absBeta = FastMath.abs(context.getBeta());
  95.             context.setHalfSpan(context.inSunSide() ?
  96.                                 absBeta * FastMath.sqrt(aNoon / absBeta - 1.0) :
  97.                                 context.inOrbitPlaneAbsoluteAngle(aNight - FastMath.PI));
  98.             if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {

  100.                 // we need to ensure beta sign does not change during the turn
  101.                 final double beta     = context.getSecuredBeta();
  102.                 final double phiStart = context.getYawStart(beta);
  103.                 final double dtStart  = context.timeSinceTurnStart(context.getDate());
  104.                 final double linearPhi;
  105.                 final double phiDot;
  106.                 if (context.inSunSide()) {
  107.                     // noon turn
  108.                     if (beta > 0 && beta < YAW_BIAS) {
  109.                         // noon turn problem for small positive beta in block IIA
  110.                         // rotation is in the wrong direction for these spacecrafts
  111.                         phiDot    = FastMath.copySign(yawRate, beta);
  112.                         linearPhi = phiStart + phiDot * dtStart;
  113.                     } else {
  114.                         // regular noon turn
  115.                         phiDot    = -FastMath.copySign(yawRate, beta);
  116.                         linearPhi = phiStart + phiDot * dtStart;
  117.                     }
  118.                 } else {
  119.                     // midnight turn
  120.                     final double dtEnd = dtStart - context.getTurnDuration();
  121.                     if (dtEnd < 0) {
  122.                         // we are within the turn itself
  123.                         phiDot    = yawRate;
  124.                         linearPhi = phiStart + phiDot * dtStart;
  125.                     } else {
  126.                         // we are in the recovery phase after turn
  127.                         phiDot = yawRate;
  128.                         final double phiEnd   = phiStart + phiDot * context.getTurnDuration();
  129.                         final double deltaPhi = context.yawAngle() - phiEnd;
  130.                         if (FastMath.abs(deltaPhi / phiDot) <= dtEnd) {
  131.                             // time since turn end was sufficient for recovery
  132.                             // we are already back in nominal yaw mode
  133.                             return context.getNominalYaw();
  134.                         } else {
  135.                             // recovery is not finished yet
  136.                             linearPhi = phiEnd + FastMath.copySign(yawRate * dtEnd, deltaPhi);
  137.                         }
  138.                     }
  139.                 }

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

  143.             }

  145.         }

  147.         // in nominal yaw mode
  148.         return context.getNominalYaw();

  150.     }

  152.     /** {@inheritDoc} */
  153.     @Override
  154.     protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {

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

  158.         // noon beta angle limit from yaw rate
  159.         final T      aNoon  = FastMath.atan(context.getMuRate().divide(yawRate));
  160.         final T      aNight = field.getZero().add(NIGHT_TURN_LIMIT);
  161.         final double cNoon  = FastMath.cos(aNoon.getReal());
  162.         final double cNight = FastMath.cos(aNight.getReal());

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

  166.             final T absBeta = FastMath.abs(context.getBeta());
  167.             context.setHalfSpan(context.inSunSide() ?
  168.                                 absBeta.multiply(FastMath.sqrt(aNoon.divide(absBeta).subtract(1.0))) :
  169.                                 context.inOrbitPlaneAbsoluteAngle(aNight.subtract(FastMath.PI)));
  170.             if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {

  172.                 // we need to ensure beta sign does not change during the turn
  173.                 final T beta     = context.getSecuredBeta();
  174.                 final T phiStart = context.getYawStart(beta);
  175.                 final T dtStart  = context.timeSinceTurnStart(context.getDate());
  176.                 final T linearPhi;
  177.                 final T phiDot;
  178.                 if (context.inSunSide()) {
  179.                     // noon turn
  180.                     if (beta.getReal() > 0 && beta.getReal() < YAW_BIAS) {
  181.                         // noon turn problem for small positive beta in block IIA
  182.                         // rotation is in the wrong direction for these spacecrafts
  183.                         phiDot    = field.getZero().add(FastMath.copySign(yawRate, beta.getReal()));
  184.                         linearPhi = phiStart.add(phiDot.multiply(dtStart));
  185.                     } else {
  186.                         // regular noon turn
  187.                         phiDot    = field.getZero().add(-FastMath.copySign(yawRate, beta.getReal()));
  188.                         linearPhi = phiStart.add(phiDot.multiply(dtStart));
  189.                     }
  190.                 } else {
  191.                     // midnight turn
  192.                     final T dtEnd = dtStart.subtract(context.getTurnDuration());
  193.                     if (dtEnd.getReal() < 0) {
  194.                         // we are within the turn itself
  195.                         phiDot    = field.getZero().add(yawRate);
  196.                         linearPhi = phiStart.add(phiDot.multiply(dtStart));
  197.                     } else {
  198.                         // we are in the recovery phase after turn
  199.                         phiDot = field.getZero().add(yawRate);
  200.                         final T phiEnd   = phiStart.add(phiDot.multiply(context.getTurnDuration()));
  201.                         final T deltaPhi = context.yawAngle().subtract(phiEnd);
  202.                         if (FastMath.abs(deltaPhi.divide(phiDot).getReal()) <= dtEnd.getReal()) {
  203.                             // time since turn end was sufficient for recovery
  204.                             // we are already back in nominal yaw mode
  205.                             return context.getNominalYaw();
  206.                         } else {
  207.                             // recovery is not finished yet
  208.                             linearPhi = phiEnd.add(dtEnd.multiply(yawRate).copySign(deltaPhi));
  209.                         }
  210.                     }
  211.                 }

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

  215.             }

  217.         }

  219.         // in nominal yaw mode
  220.         return context.getNominalYaw();

  222.     }

  224. }
Created with JaCoCo 0.7.9.201702052155