1 /* Copyright 2002-2025 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
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.util.FastMath;
21 import org.hipparchus.util.MathUtils;
22 import org.orekit.frames.Frame;
23 import org.orekit.time.AbsoluteDate;
24 import org.orekit.time.FieldAbsoluteDate;
25 import org.orekit.utils.ExtendedPositionProvider;
26 import org.orekit.utils.TimeStampedAngularCoordinates;
27 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
28
29 /**
30 * Attitude providers for Beidou Medium Earth Orbit navigation satellites.
31 * @author Luc Maisonobe Java translation
32 * @since 9.2
33 */
34 public class BeidouMeo extends AbstractGNSSAttitudeProvider {
35
36 /** Limit for the Yaw Steering to Orbit Normal switch. */
37 private static final double BETA_YS_ON = FastMath.toRadians(4.1);
38
39 /** Limit for the Orbit Normal to Yaw Steering switch. */
40 private static final double BETA_ON_YS = FastMath.toRadians(3.9);
41
42 /** Simple constructor.
43 * @param validityStart start of validity for this provider
44 * @param validityEnd end of validity for this provider
45 * @param sun provider for Sun position
46 * @param inertialFrame inertial frame where velocity are computed
47 */
48 public BeidouMeo(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
49 final ExtendedPositionProvider sun, final Frame inertialFrame) {
50 super(validityStart, validityEnd, sun, inertialFrame);
51 }
52
53 /** {@inheritDoc} */
54 @Override
55 protected TimeStampedAngularCoordinates correctedYaw(final GNSSAttitudeContext context) {
56
57 // variation of the β angle over one orbital period (approximately)
58 final double beta = context.beta(context.getDate());
59 final double approxPeriod = 2 * FastMath.PI / context.getMuRate();
60 final double betaVariation = beta - context.beta(context.getDate().shiftedBy(-approxPeriod));
61 final double delta = context.getOrbitAngleSinceMidnight();
62
63 if (FastMath.abs(beta) <= BETA_YS_ON - FastMath.abs(betaVariation)) {
64 // the β angle is lower than threshold for a complete orbital period
65 // we are for sure in the Orbit Normal (ON) mode
66 return context.orbitNormalYaw();
67 } else if (FastMath.abs(beta) > BETA_ON_YS + FastMath.abs(betaVariation)) {
68 // the β angle is higher than threshold for a complete orbital period,
69 // we are for sure in the Yaw Steering mode
70 return context.nominalYaw(context.getDate());
71 } else {
72 // we are in the grey zone, somewhere near a mode switch
73 final boolean absBetaDecreasing = beta * betaVariation <= 0.0;
74
75 if (absBetaDecreasing) {
76 // we are going towards the β = 0 limit
77 if (FastMath.abs(beta) >= BETA_YS_ON) {
78 // we have not yet reached the far limit, we are still in Yaw Steering
79 return context.nominalYaw(context.getDate());
80 }
81 } else {
82 // we are going away from the β = 0 limit
83 if (FastMath.abs(beta) <= BETA_ON_YS) {
84 // we have not yet reached the close limit, we are still in Orbit Normal
85 return context.orbitNormalYaw();
86 }
87 }
88
89 // there is a mode switch near the current orbit, it occurs when orbit angle is 90°
90 // we check what was the β angle at the previous quadrature to see if the switch
91 // already occurred
92 final double angleSinceQuadrature =
93 MathUtils.normalizeAngle(delta - 0.5 * FastMath.PI, FastMath.PI);
94 final double timeSinceQuadrature = angleSinceQuadrature / context.getMuRate();
95 final AbsoluteDate quadratureDate = context.getDate().shiftedBy(-timeSinceQuadrature);
96 final double betaQuadrature = context.beta(quadratureDate);
97
98 if (absBetaDecreasing) {
99 // we are going towards the β = 0 limit
100 if (FastMath.abs(betaQuadrature) <= BETA_YS_ON) {
101 // we have switched to Orbit Normal mode since last quadrature
102 return context.orbitNormalYaw();
103 }
104 } else {
105 // we are going away from the β = 0 limit
106 if (FastMath.abs(betaQuadrature) <= BETA_ON_YS) {
107 // β was below switch at last quadrature, we are still in the Orbit Normal mode
108 return context.orbitNormalYaw();
109 }
110 }
111
112 return context.nominalYaw(context.getDate());
113
114 }
115
116 }
117
118 /** {@inheritDoc} */
119 @Override
120 protected <T extends CalculusFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {
121
122 // variation of the β angle over one orbital period (approximately)
123 final double beta = context.beta(context.getDate()).getReal();
124 final double approxPeriod = 2 * FastMath.PI / context.getMuRate().getReal();
125 final double betaVariation = beta - context.beta(context.getDate().shiftedBy(-approxPeriod)).getReal();
126 final double delta = context.getOrbitAngleSinceMidnight().getReal();
127
128 if (FastMath.abs(beta) <= BETA_YS_ON - FastMath.abs(betaVariation)) {
129 // the β angle is lower than threshold for a complete orbital period
130 // we are for sure in the Orbit Normal (ON) mode
131 return context.orbitNormalYaw();
132 } else if (FastMath.abs(beta) > BETA_ON_YS + FastMath.abs(betaVariation)) {
133 // the β angle is higher than threshold for a complete orbital period,
134 // we are for sure in the Yaw Steering mode
135 return context.nominalYaw(context.getDate());
136 } else {
137 // we are in the grey zone, somewhere near a mode switch
138 final boolean absBetaDecreasing = beta * betaVariation <= 0.0;
139
140 if (absBetaDecreasing) {
141 // we are going towards the β = 0 limit
142 if (FastMath.abs(beta) >= BETA_YS_ON) {
143 // we have not yet reached the far limit, we are still in Yaw Steering
144 return context.nominalYaw(context.getDate());
145 }
146 } else {
147 // we are going away from the β = 0 limit
148 if (FastMath.abs(beta) <= BETA_ON_YS) {
149 // we have not yet reached the close limit, we are still in Orbit Normal
150 return context.orbitNormalYaw();
151 }
152 }
153
154 // there is a mode switch near the current orbit, it occurs when orbit angle is 90°
155 // we check what was the β angle at the previous quadrature to see if the switch
156 // already occurred
157 final double angleSinceQuadrature =
158 MathUtils.normalizeAngle(delta - 0.5 * FastMath.PI, FastMath.PI);
159 final double timeSinceQuadrature = angleSinceQuadrature / context.getMuRate().getReal();
160 final FieldAbsoluteDate<T> quadratureDate = context.getDate().shiftedBy(-timeSinceQuadrature);
161 final double betaQuadrature = context.beta(quadratureDate).getReal();
162
163 if (absBetaDecreasing) {
164 // we are going towards the β = 0 limit
165 if (FastMath.abs(betaQuadrature) <= BETA_YS_ON) {
166 // we have switched to Orbit Normal mode since last quadrature
167 return context.orbitNormalYaw();
168 }
169 } else {
170 // we are going away from the β = 0 limit
171 if (FastMath.abs(betaQuadrature) <= BETA_ON_YS) {
172 // β was below switch at last quadrature, we are still in the Orbit Normal mode
173 return context.orbitNormalYaw();
174 }
175 }
176
177 return context.nominalYaw(context.getDate());
178
179 }
180
181 }
182
183 }