BeidouMeo.java

  1. /* Copyright 2002-2024 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.CalculusFieldElement;
  19. import org.hipparchus.util.FastMath;
  20. import org.hipparchus.util.MathUtils;
  21. import org.orekit.frames.Frame;
  22. import org.orekit.time.AbsoluteDate;
  23. import org.orekit.time.FieldAbsoluteDate;
  24. import org.orekit.utils.ExtendedPVCoordinatesProvider;
  25. import org.orekit.utils.TimeStampedAngularCoordinates;
  26. import org.orekit.utils.TimeStampedFieldAngularCoordinates;

  27. /**
  28.  * Attitude providers for Beidou Medium Earth Orbit navigation satellites.
  29.  * @author Luc Maisonobe Java translation
  30.  * @since 9.2
  31.  */
  32. public class BeidouMeo extends AbstractGNSSAttitudeProvider {

  33.     /** Limit for the Yaw Steering to Orbit Normal switch. */
  34.     private static final double BETA_YS_ON = FastMath.toRadians(4.1);

  35.     /** Limit for the Orbit Normal to Yaw Steering switch. */
  36.     private static final double BETA_ON_YS = FastMath.toRadians(3.9);

  37.     /** Simple constructor.
  38.      * @param validityStart start of validity for this provider
  39.      * @param validityEnd end of validity for this provider
  40.      * @param sun provider for Sun position
  41.      * @param inertialFrame inertial frame where velocity are computed
  42.      */
  43.     public BeidouMeo(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
  44.                      final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
  45.         super(validityStart, validityEnd, sun, inertialFrame);
  46.     }

  47.     /** {@inheritDoc} */
  48.     @Override
  49.     protected TimeStampedAngularCoordinates correctedYaw(final GNSSAttitudeContext context) {

  50.         // variation of the β angle over one orbital period (approximately)
  51.         final double beta          = context.beta(context.getDate());
  52.         final double approxPeriod  = 2 * FastMath.PI / context.getMuRate();
  53.         final double betaVariation = beta - context.beta(context.getDate().shiftedBy(-approxPeriod));
  54.         final double delta         = context.getOrbitAngleSinceMidnight();

  55.         if (FastMath.abs(beta) <= BETA_YS_ON - FastMath.abs(betaVariation)) {
  56.             // the β angle is lower than threshold for a complete orbital period
  57.             // we are for sure in the Orbit Normal (ON) mode
  58.             return context.orbitNormalYaw();
  59.         } else if (FastMath.abs(beta) > BETA_ON_YS + FastMath.abs(betaVariation)) {
  60.             // the β angle is higher than threshold for a complete orbital period,
  61.             // we are for sure in the Yaw Steering mode
  62.             return context.nominalYaw(context.getDate());
  63.         } else {
  64.             // we are in the grey zone, somewhere near a mode switch
  65.             final boolean absBetaDecreasing = beta * betaVariation <= 0.0;

  66.             if (absBetaDecreasing) {
  67.                 // we are going towards the β = 0 limit
  68.                 if (FastMath.abs(beta) >= BETA_YS_ON) {
  69.                     // we have not yet reached the far limit, we are still in Yaw Steering
  70.                     return context.nominalYaw(context.getDate());
  71.                 }
  72.             } else {
  73.                 // we are going away from the β = 0 limit
  74.                 if (FastMath.abs(beta) <= BETA_ON_YS) {
  75.                     // we have not yet reached the close limit, we are still in Orbit Normal
  76.                     return context.orbitNormalYaw();
  77.                 }
  78.             }

  79.             // there is a mode switch near the current orbit, it occurs when orbit angle is 90°
  80.             // we check what was the β angle at the previous quadrature to see if the switch
  81.             // already occurred
  82.             final double angleSinceQuadrature =
  83.                             MathUtils.normalizeAngle(delta - 0.5 * FastMath.PI, FastMath.PI);
  84.             final double timeSinceQuadrature = angleSinceQuadrature / context.getMuRate();
  85.             final AbsoluteDate quadratureDate = context.getDate().shiftedBy(-timeSinceQuadrature);
  86.             final double betaQuadrature = context.beta(quadratureDate);

  87.             if (absBetaDecreasing) {
  88.                 // we are going towards the β = 0 limit
  89.                 if (FastMath.abs(betaQuadrature) <= BETA_YS_ON) {
  90.                     // we have switched to Orbit Normal mode since last quadrature
  91.                     return context.orbitNormalYaw();
  92.                 }
  93.             } else {
  94.                 // we are going away from the β = 0 limit
  95.                 if (FastMath.abs(betaQuadrature) <= BETA_ON_YS) {
  96.                     // β was below switch at last quadrature, we are still in the Orbit Normal mode
  97.                     return context.orbitNormalYaw();
  98.                 }
  99.             }

  100.             return context.nominalYaw(context.getDate());

  101.         }

  102.     }

  103.     /** {@inheritDoc} */
  104.     @Override
  105.     protected <T extends CalculusFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {

  106.         // variation of the β angle over one orbital period (approximately)
  107.         final double beta          = context.beta(context.getDate()).getReal();
  108.         final double approxPeriod  = 2 * FastMath.PI / context.getMuRate().getReal();
  109.         final double betaVariation = beta - context.beta(context.getDate().shiftedBy(-approxPeriod)).getReal();
  110.         final double delta         = context.getOrbitAngleSinceMidnight().getReal();

  111.         if (FastMath.abs(beta) <= BETA_YS_ON - FastMath.abs(betaVariation)) {
  112.             // the β angle is lower than threshold for a complete orbital period
  113.             // we are for sure in the Orbit Normal (ON) mode
  114.             return context.orbitNormalYaw();
  115.         } else if (FastMath.abs(beta) > BETA_ON_YS + FastMath.abs(betaVariation)) {
  116.             // the β angle is higher than threshold for a complete orbital period,
  117.             // we are for sure in the Yaw Steering mode
  118.             return context.nominalYaw(context.getDate());
  119.         } else {
  120.             // we are in the grey zone, somewhere near a mode switch
  121.             final boolean absBetaDecreasing = beta * betaVariation <= 0.0;

  122.             if (absBetaDecreasing) {
  123.                 // we are going towards the β = 0 limit
  124.                 if (FastMath.abs(beta) >= BETA_YS_ON) {
  125.                     // we have not yet reached the far limit, we are still in Yaw Steering
  126.                     return context.nominalYaw(context.getDate());
  127.                 }
  128.             } else {
  129.                 // we are going away from the β = 0 limit
  130.                 if (FastMath.abs(beta) <= BETA_ON_YS) {
  131.                     // we have not yet reached the close limit, we are still in Orbit Normal
  132.                     return context.orbitNormalYaw();
  133.                 }
  134.             }

  135.             // there is a mode switch near the current orbit, it occurs when orbit angle is 90°
  136.             // we check what was the β angle at the previous quadrature to see if the switch
  137.             // already occurred
  138.             final double angleSinceQuadrature =
  139.                             MathUtils.normalizeAngle(delta - 0.5 * FastMath.PI, FastMath.PI);
  140.             final double timeSinceQuadrature = angleSinceQuadrature / context.getMuRate().getReal();
  141.             final FieldAbsoluteDate<T> quadratureDate = context.getDate().shiftedBy(-timeSinceQuadrature);
  142.             final double betaQuadrature = context.beta(quadratureDate).getReal();

  143.             if (absBetaDecreasing) {
  144.                 // we are going towards the β = 0 limit
  145.                 if (FastMath.abs(betaQuadrature) <= BETA_YS_ON) {
  146.                     // we have switched to Orbit Normal mode since last quadrature
  147.                     return context.orbitNormalYaw();
  148.                 }
  149.             } else {
  150.                 // we are going away from the β = 0 limit
  151.                 if (FastMath.abs(betaQuadrature) <= BETA_ON_YS) {
  152.                     // β was below switch at last quadrature, we are still in the Orbit Normal mode
  153.                     return context.orbitNormalYaw();
  154.                 }
  155.             }

  156.             return context.nominalYaw(context.getDate());

  157.         }

  158.     }

  159. }