1   /* Copyright 2002-2019 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;
18  
19  import org.hipparchus.Field;
20  import org.hipparchus.RealFieldElement;
21  import org.hipparchus.analysis.differentiation.DerivativeStructure;
22  import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
23  import org.hipparchus.util.FastMath;
24  import org.orekit.frames.Frame;
25  import org.orekit.time.AbsoluteDate;
26  import org.orekit.utils.ExtendedPVCoordinatesProvider;
27  import org.orekit.utils.TimeStampedAngularCoordinates;
28  import org.orekit.utils.TimeStampedFieldAngularCoordinates;
29  
30  /**
31   * Attitude providers for Galileo navigation satellites.
32   * <p>
33   * This class is based on the May 2017 version of J. Kouba eclips.f
34   * subroutine available at <a href="http://acc.igs.org/orbits">IGS Analysis
35   * Center Coordinator site</a>. The eclips.f code itself is not used ; its
36   * hard-coded data are used and its low level models are used, but the
37   * structure of the code and the API have been completely rewritten.
38   * </p>
39   * @author J. Kouba original fortran routine
40   * @author Luc Maisonobe Java translation
41   * @since 9.2
42   */
43  public class Galileo extends AbstractGNSSAttitudeProvider {
44  
45      /** Default yaw rates for all spacecrafts in radians per seconds. */
46      public static final double DEFAULT_YAW_RATE = FastMath.toRadians(0.203);
47  
48      /** Serializable UID. */
49      private static final long serialVersionUID = 20171114L;
50  
51      /** Constants for Galileo turns. */
52      private static final double BETA_X = FastMath.toRadians(15.0);
53  
54      /** Limit for the noon turn. */
55      private static final double COS_NOON = FastMath.cos(BETA_X);
56  
57      /** Limit for the night turn. */
58      private static final double COS_NIGHT = -COS_NOON;
59  
60      /** No margin on turn end for Galileo. */
61      private final double END_MARGIN = 0.0;
62  
63      /** Yaw rate. */
64      private final double yawRate;
65  
66      /** Simple constructor.
67       * @param yawRate yaw rate to use in radians per seconds (typically {@link #DEFAULT_YAW_RATE})
68       * @param validityStart start of validity for this provider
69       * @param validityEnd end of validity for this provider
70       * @param sun provider for Sun position
71       * @param inertialFrame inertial frame where velocity are computed
72       */
73      public Galileo(final double yawRate,
74                     final AbsoluteDateDate">AbsoluteDate validityStart, final AbsoluteDate validityEnd,
75                     final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
76          super(validityStart, validityEnd, sun, inertialFrame);
77          this.yawRate = yawRate;
78      }
79  
80      /** {@inheritDoc} */
81      @Override
82      protected TimeStampedAngularCoordinates correctedYaw(final GNSSAttitudeContext context) {
83  
84          // noon beta angle limit from yaw rate
85          final double beta0 = FastMath.atan(context.getMuRate() / yawRate);
86  
87          if (FastMath.abs(context.beta(context.getDate())) < beta0 &&
88              context.setUpTurnRegion(COS_NIGHT, COS_NOON)) {
89  
90              context.setHalfSpan(context.inSunSide() ?
91                                  BETA_X :
92                                  context.inOrbitPlaneAbsoluteAngle(BETA_X),
93                                  END_MARGIN);
94              if (context.inTurnTimeRange()) {
95  
96                  // handling both noon and midnight turns at once
97                  final DerivativeStructure beta     = context.betaDS();
98                  final DerivativeStructure cosBeta  = beta.cos();
99                  final DerivativeStructure sinBeta  = beta.sin();
100                 final double              sinY     = FastMath.copySign(FastMath.sin(beta0), context.getSecuredBeta());
101                 final DerivativeStructure sd       = FastMath.sin(context.getDeltaDS()).
102                                                      multiply(FastMath.copySign(1.0, -context.getSVBcos() * context.getDeltaDS().getPartialDerivative(1)));
103                 final DerivativeStructure c        = sd.multiply(cosBeta);
104                 final DerivativeStructure shy      = sinBeta.negate().subtract(sinY).
105                                                      add(sinBeta.subtract(sinY).multiply(c.abs().multiply(FastMath.PI / FastMath.sin(BETA_X)).cos())).
106                                                      multiply(0.5);
107                 final DerivativeStructure phi      = FastMath.atan2(shy, c);
108 
109                 return context.turnCorrectedAttitude(phi);
110 
111             }
112 
113         }
114 
115         // in nominal yaw mode
116         return context.nominalYaw(context.getDate());
117 
118     }
119 
120     /** {@inheritDoc} */
121     @Override
122     protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {
123 
124         // noon beta angle limit from yaw rate
125         final double beta0 = FastMath.atan(context.getMuRate().getReal() / yawRate);
126 
127         if (FastMath.abs(context.beta(context.getDate())).getReal() < beta0 &&
128             context.setUpTurnRegion(COS_NIGHT, COS_NOON)) {
129 
130             final Field<T> field = context.getDate().getField();
131             final T        betaX = field.getZero().add(BETA_X);
132             context.setHalfSpan(context.inSunSide() ?
133                                 betaX :
134                                 context.inOrbitPlaneAbsoluteAngle(betaX),
135                                 END_MARGIN);
136             if (context.inTurnTimeRange()) {
137 
138                 // handling both noon and midnight turns at once
139                 final FieldDerivativeStructure<T> beta     = context.betaDS();
140                 final FieldDerivativeStructure<T> cosBeta  = beta.cos();
141                 final FieldDerivativeStructure<T> sinBeta  = beta.sin();
142                 final T                           sinY     = FastMath.sin(field.getZero().add(beta0)).copySign(context.getSecuredBeta());
143                 final FieldDerivativeStructure<T> sd       = FastMath.sin(context.getDeltaDS()).
144                                                              multiply(FastMath.copySign(1.0, -context.getSVBcos().getReal() * context.getDeltaDS().getPartialDerivative(1).getReal()));
145                 final FieldDerivativeStructure<T> c        = sd.multiply(cosBeta);
146                 final FieldDerivativeStructure<T> shy      = sinBeta.negate().subtract(sinY).
147                                                              add(sinBeta.subtract(sinY).multiply(c.abs().multiply(FastMath.PI / FastMath.sin(BETA_X)).cos())).
148                                                              multiply(0.5);
149                 final FieldDerivativeStructure<T> phi      = FastMath.atan2(shy, c);
150 
151                 return context.turnCorrectedAttitude(phi);
152 
153             }
154 
155         }
156 
157         // in nominal yaw mode
158         return context.nominalYaw(context.getDate());
159 
160     }
161 
162 }