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;
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   * <p>
40   * WARNING: as of release 9.2, this feature is still considered experimental
41   * </p>
42   * @author J. Kouba original fortran routine
43   * @author Luc Maisonobe Java translation
44   * @since 9.2
45   */
46  public class Galileo extends AbstractGNSSAttitudeProvider {
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      /** Constants for Galileo turns. */
55      private static final double BETA_Y = FastMath.toRadians(2.0);
56  
57      /** Limit for the noon turn. */
58      private static final double COS_NOON = FastMath.cos(BETA_X);
59  
60      /** Limit for the night turn. */
61      private static final double COS_NIGHT = -COS_NOON;
62  
63      /** No margin on turn end for Galileo. */
64      private final double END_MARGIN = 0.0;
65  
66      /** Simple constructor.
67       * @param validityStart start of validity for this provider
68       * @param validityEnd end of validity for this provider
69       * @param sun provider for Sun position
70       * @param inertialFrame inertial frame where velocity are computed
71       */
72      public Galileo(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
73                     final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
74          super(validityStart, validityEnd, sun, inertialFrame);
75      }
76  
77      /** {@inheritDoc} */
78      @Override
79      protected TimeStampedAngularCoordinates correctedYaw(final GNSSAttitudeContext context) {
80  
81          if (FastMath.abs(context.getBeta()) < BETA_Y &&
82              context.setUpTurnRegion(COS_NIGHT, COS_NOON)) {
83  
84              context.setHalfSpan(context.inSunSide() ?
85                                  BETA_X :
86                                  context.inOrbitPlaneAbsoluteAngle(BETA_X));
87              if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
88  
89                  // handling both noon and midnight turns at once
90                  final DerivativeStructure beta     = context.getBetaDS();
91                  final DerivativeStructure cosBeta  = beta.cos();
92                  final DerivativeStructure sinBeta  = beta.sin();
93                  final double              sinY     = FastMath.copySign(FastMath.sin(BETA_Y), context.getSecuredBeta());
94                  final DerivativeStructure sd       = FastMath.sin(context.getDeltaDS()).
95                                                       multiply(FastMath.copySign(1.0, -context.getSVBcos() * context.getDeltaDS().getPartialDerivative(1)));
96                  final DerivativeStructure c        = sd.multiply(cosBeta);
97                  final DerivativeStructure shy      = sinBeta.negate().subtract(sinY).
98                                                       add(sinBeta.subtract(sinY).multiply(c.abs().multiply(FastMath.PI / FastMath.sin(BETA_X)).cos())).
99                                                       multiply(0.5);
100                 final DerivativeStructure phi     = FastMath.atan2(shy, c);
101 
102                 return context.turnCorrectedAttitude(phi);
103 
104             }
105 
106         }
107 
108         // in nominal yaw mode
109         return context.getNominalYaw();
110 
111     }
112 
113     /** {@inheritDoc} */
114     @Override
115     protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {
116 
117         if (FastMath.abs(context.getBeta()).getReal() < BETA_Y &&
118             context.setUpTurnRegion(COS_NIGHT, COS_NOON)) {
119 
120             final Field<T> field = context.getDate().getField();
121             final T        betaX = field.getZero().add(BETA_X);
122             context.setHalfSpan(context.inSunSide() ?
123                                 betaX :
124                                 context.inOrbitPlaneAbsoluteAngle(betaX));
125             if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
126 
127                 // handling both noon and midnight turns at once
128                 final FieldDerivativeStructure<T> beta     = context.getBetaDS();
129                 final FieldDerivativeStructure<T> cosBeta  = beta.cos();
130                 final FieldDerivativeStructure<T> sinBeta  = beta.sin();
131                 final T                           sinY     = FastMath.sin(field.getZero().add(BETA_Y)).copySign(context.getSecuredBeta());
132                 final FieldDerivativeStructure<T> sd       = FastMath.sin(context.getDeltaDS()).
133                                                              multiply(FastMath.copySign(1.0, -context.getSVBcos().getReal() * context.getDeltaDS().getPartialDerivative(1).getReal()));
134                 final FieldDerivativeStructure<T> c        = sd.multiply(cosBeta);
135                 final FieldDerivativeStructure<T> shy      = sinBeta.negate().subtract(sinY).
136                                                              add(sinBeta.subtract(sinY).multiply(c.abs().multiply(FastMath.PI / FastMath.sin(BETA_X)).cos())).
137                                                              multiply(0.5);
138                 final FieldDerivativeStructure<T> phi     = FastMath.atan2(shy, c);
139 
140                 return context.turnCorrectedAttitude(phi);
141 
142             }
143 
144         }
145 
146         // in nominal yaw mode
147         return context.getNominalYaw();
148 
149     }
150 
151 }