GPSBlockIIF.java

  1. /* Copyright 2002-2020 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;

  18. import org.hipparchus.Field;
  19. import org.hipparchus.RealFieldElement;
  20. import org.hipparchus.util.FastMath;
  21. import org.orekit.frames.Frame;
  22. import org.orekit.time.AbsoluteDate;
  23. import org.orekit.utils.ExtendedPVCoordinatesProvider;
  24. import org.orekit.utils.TimeStampedAngularCoordinates;
  25. import org.orekit.utils.TimeStampedFieldAngularCoordinates;

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

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

  42.     /** Default yaw bias (rad). */
  43.     public static final double DEFAULT_YAW_BIAS = FastMath.toRadians(-0.7);

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

  46.     /** Margin on turn end. */
  47.     private static final double END_MARGIN = 1800.0;

  48.     /** Yaw rate. */
  49.     private final double yawRate;

  50.     /** Yaw bias. */
  51.     private final double yawBias;

  52.     /** Simple constructor.
  53.      * @param yawRate yaw rate to use in radians per seconds (typically {@link #DEFAULT_YAW_RATE})
  54.      * @param yawBias yaw bias to use (rad) (typicall {@link #DEFAULT_YAW_BIAS})
  55.      * @param validityStart start of validity for this provider
  56.      * @param validityEnd end of validity for this provider
  57.      * @param sun provider for Sun position
  58.      * @param inertialFrame inertial frame where velocity are computed
  59.      */
  60.     public GPSBlockIIF(final double yawRate, final double yawBias,
  61.                        final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
  62.                        final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
  63.         super(validityStart, validityEnd, sun, inertialFrame);
  64.         this.yawRate = yawRate;
  65.         this.yawBias = yawBias;
  66.     }

  67.     /** {@inheritDoc} */
  68.     @Override
  69.     protected TimeStampedAngularCoordinates correctedYaw(final GNSSAttitudeContext context) {

  70.         // noon beta angle limit from yaw rate
  71.         final double aNoon  = FastMath.atan(context.getMuRate() / yawRate);
  72.         final double aNight = NIGHT_TURN_LIMIT;
  73.         final double cNoon  = FastMath.cos(aNoon);
  74.         final double cNight = FastMath.cos(aNight);

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

  76.             final double absBeta = FastMath.abs(context.beta(context.getDate()));
  77.             context.setHalfSpan(context.inSunSide() ?
  78.                                 absBeta * FastMath.sqrt(aNoon / absBeta - 1.0) :
  79.                                 context.inOrbitPlaneAbsoluteAngle(aNight - FastMath.PI),
  80.                                 END_MARGIN);
  81.             if (context.inTurnTimeRange()) {

  82.                 // we need to ensure beta sign does not change during the turn
  83.                 final double beta     = context.getSecuredBeta();
  84.                 final double phiStart = context.getYawStart(beta);
  85.                 final double dtStart  = context.timeSinceTurnStart();
  86.                 final double phiDot;
  87.                 final double linearPhi;
  88.                 if (context.inSunSide()) {
  89.                     // noon turn
  90.                     if (beta > yawBias && beta < 0) {
  91.                         // noon turn problem for small negative beta in block IIF
  92.                         // rotation is in the wrong direction for these spacecrafts
  93.                         phiDot    = FastMath.copySign(yawRate, beta);
  94.                         linearPhi = phiStart + phiDot * dtStart;
  95.                     } else {
  96.                         // regular noon turn
  97.                         phiDot    = -FastMath.copySign(yawRate, beta);
  98.                         linearPhi = phiStart + phiDot * dtStart;
  99.                     }
  100.                 } else {
  101.                     // midnight turn
  102.                     phiDot    = context.yawRate(beta);
  103.                     linearPhi = phiStart + phiDot * dtStart;
  104.                 }

  105.                 if (context.linearModelStillActive(linearPhi, phiDot)) {
  106.                     // we are still in the linear model phase
  107.                     return context.turnCorrectedAttitude(linearPhi, phiDot);
  108.                 }


  109.             }

  110.         }

  111.         // in nominal yaw mode
  112.         return context.nominalYaw(context.getDate());

  113.     }

  114.     /** {@inheritDoc} */
  115.     @Override
  116.     protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {

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

  118.         // noon beta angle limit from yaw rate
  119.         final T      aNoon  = FastMath.atan(context.getMuRate().divide(yawRate));
  120.         final T      aNight = field.getZero().add(NIGHT_TURN_LIMIT);
  121.         final double cNoon  = FastMath.cos(aNoon.getReal());
  122.         final double cNight = FastMath.cos(aNight.getReal());

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

  124.             final T absBeta = FastMath.abs(context.beta(context.getDate()));
  125.             context.setHalfSpan(context.inSunSide() ?
  126.                                 absBeta.multiply(FastMath.sqrt(aNoon.divide(absBeta).subtract(1.0))) :
  127.                                 context.inOrbitPlaneAbsoluteAngle(aNight.subtract(FastMath.PI)),
  128.                                 END_MARGIN);
  129.             if (context.inTurnTimeRange()) {

  130.                 // we need to ensure beta sign does not change during the turn
  131.                 final T beta     = context.getSecuredBeta();
  132.                 final T phiStart = context.getYawStart(beta);
  133.                 final T dtStart  = context.timeSinceTurnStart();
  134.                 final T phiDot;
  135.                 final T linearPhi;
  136.                 if (context.inSunSide()) {
  137.                     // noon turn
  138.                     if (beta.getReal() > yawBias && beta.getReal() < 0) {
  139.                         // noon turn problem for small negative beta in block IIF
  140.                         // rotation is in the wrong direction for these spacecrafts
  141.                         phiDot    = field.getZero().add(FastMath.copySign(yawRate, beta.getReal()));
  142.                         linearPhi = phiStart.add(phiDot.multiply(dtStart));
  143.                     } else {
  144.                         // regular noon turn
  145.                         phiDot    = field.getZero().add(-FastMath.copySign(yawRate, beta.getReal()));
  146.                         linearPhi = phiStart.add(phiDot.multiply(dtStart));
  147.                     }
  148.                 } else {
  149.                     // midnight turn
  150.                     phiDot    = context.yawRate(beta);
  151.                     linearPhi = phiStart.add(phiDot.multiply(dtStart));
  152.                 }

  153.                 if (context.linearModelStillActive(linearPhi, phiDot)) {
  154.                     // we are still in the linear model phase
  155.                     return context.turnCorrectedAttitude(linearPhi, phiDot);
  156.                 }

  157.             }

  158.         }

  159.         // in nominal yaw mode
  160.         return context.nominalYaw(context.getDate());

  161.     }

  162. }