GPSBlockIIR.java

  1. /* Copyright 2002-2022 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.CalculusFieldElement;
  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 IIR 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 GPSBlockIIR extends AbstractGNSSAttitudeProvider {

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

  42.     /** Margin on turn end. */
  43.     private static final double END_MARGIN = 1800.0;

  44.     /** Yaw rate. */
  45.     private final double yawRate;

  46.     /** Simple constructor.
  47.      * @param yawRate yaw rate to use in radians per seconds (typically {@link #DEFAULT_YAW_RATE})
  48.      * @param validityStart start of validity for this provider
  49.      * @param validityEnd end of validity for this provider
  50.      * @param sun provider for Sun position
  51.      * @param inertialFrame inertial frame where velocity are computed
  52.      */
  53.     public GPSBlockIIR(final double yawRate,
  54.                        final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
  55.                        final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
  56.         super(validityStart, validityEnd, sun, inertialFrame);
  57.         this.yawRate = yawRate;
  58.     }

  59.     /** {@inheritDoc} */
  60.     @Override
  61.     protected TimeStampedAngularCoordinates correctedYaw(final GNSSAttitudeContext context) {

  62.         // noon beta angle limit from yaw rate
  63.         final double aNoon  = FastMath.atan(context.getMuRate() / yawRate);
  64.         final double cNoon  = FastMath.cos(aNoon);
  65.         final double cNight = -cNoon;

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

  67.             final double absBeta = FastMath.abs(context.beta(context.getDate()));
  68.             context.setHalfSpan(absBeta * FastMath.sqrt(aNoon / absBeta - 1.0), END_MARGIN);
  69.             if (context.inTurnTimeRange()) {

  70.                 // we need to ensure beta sign does not change during the turn
  71.                 final double beta     = context.getSecuredBeta();
  72.                 final double phiStart = context.getYawStart(beta);
  73.                 final double dtStart  = context.timeSinceTurnStart();
  74.                 final double phiDot;
  75.                 final double linearPhi;

  76.                 if (context.inSunSide()) {
  77.                     // noon turn
  78.                     phiDot    = -FastMath.copySign(yawRate, beta);
  79.                     linearPhi = phiStart + phiDot * dtStart;
  80.                 } else {
  81.                     // midnight turn
  82.                     phiDot    = FastMath.copySign(yawRate, beta);
  83.                     linearPhi = phiStart + phiDot * dtStart;
  84.                 }

  85.                 if (context.linearModelStillActive(linearPhi, phiDot)) {
  86.                     // we are still in the linear model phase
  87.                     return context.turnCorrectedAttitude(linearPhi, phiDot);
  88.                 }

  89.             }

  90.         }

  91.         // in nominal yaw mode
  92.         return context.nominalYaw(context.getDate());

  93.     }

  94.     /** {@inheritDoc} */
  95.     @Override
  96.     protected <T extends CalculusFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {

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

  98.         // noon beta angle limit from yaw rate
  99.         final T      aNoon  = FastMath.atan(context.getMuRate().divide(yawRate));
  100.         final double cNoon  = FastMath.cos(aNoon.getReal());
  101.         final double cNight = -cNoon;

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

  103.             final T absBeta = FastMath.abs(context.beta(context.getDate()));
  104.             context.setHalfSpan(absBeta.multiply(FastMath.sqrt(aNoon.divide(absBeta).subtract(1.0))), END_MARGIN);
  105.             if (context.inTurnTimeRange()) {

  106.                 // we need to ensure beta sign does not change during the turn
  107.                 final T beta     = context.getSecuredBeta();
  108.                 final T phiStart = context.getYawStart(beta);
  109.                 final T dtStart  = context.timeSinceTurnStart();
  110.                 final T phiDot;
  111.                 final T linearPhi;

  112.                 if (context.inSunSide()) {
  113.                     // noon turn
  114.                     phiDot    = field.getZero().add(-FastMath.copySign(yawRate, beta.getReal()));
  115.                     linearPhi = phiStart.add(phiDot.multiply(dtStart));
  116.                 } else {
  117.                     // midnight turn
  118.                     phiDot    = field.getZero().add(FastMath.copySign(yawRate, beta.getReal()));
  119.                     linearPhi = phiStart.add(phiDot.multiply(dtStart));
  120.                 }

  121.                 if (context.linearModelStillActive(linearPhi, phiDot)) {
  122.                     // we are still in the linear model phase
  123.                     return context.turnCorrectedAttitude(linearPhi, phiDot);
  124.                 }

  125.             }

  126.         }

  127.         // in nominal yaw mode
  128.         return context.nominalYaw(context.getDate());

  129.     }

  130. }