1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.gnss.attitude;
18
19 import org.hipparchus.Field;
20 import org.hipparchus.RealFieldElement;
21 import org.hipparchus.util.FastMath;
22 import org.orekit.frames.Frame;
23 import org.orekit.time.AbsoluteDate;
24 import org.orekit.utils.ExtendedPVCoordinatesProvider;
25 import org.orekit.utils.TimeStampedAngularCoordinates;
26 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44 public class GPSBlockIIA extends AbstractGNSSAttitudeProvider {
45
46
47 private static final long serialVersionUID = 20171114L;
48
49
50 private static final double NIGHT_TURN_LIMIT = FastMath.toRadians(180.0 - 13.25);
51
52
53 private static final double YAW_BIAS = FastMath.toRadians(0.5);
54
55
56 private static final double[] YAW_RATES = new double[] {
57 0.1211, 0.1339, 0.1230, 0.1233, 0.1180, 0.1266, 0.1269, 0.1033,
58 0.1278, 0.0978, 0.2000, 0.1990, 0.2000, 0.0815, 0.1303, 0.0838,
59 0.1401, 0.1069, 0.0980, 0.1030, 0.1366, 0.1025, 0.1140, 0.1089,
60 0.1001, 0.1227, 0.1194, 0.1260, 0.1228, 0.1165, 0.0969, 0.1140
61 };
62
63
64 private final double END_MARGIN = 1800.0;
65
66
67 private final double yawRate;
68
69
70
71
72
73
74
75
76 public GPSBlockIIA(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
77 final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame, final int prnNumber) {
78 super(validityStart, validityEnd, sun, inertialFrame);
79 yawRate = FastMath.toRadians(YAW_RATES[prnNumber - 1]);
80 }
81
82
83 @Override
84 protected TimeStampedAngularCoordinates correctedYaw(final GNSSAttitudeContext context) {
85
86
87 final double aNoon = FastMath.atan(context.getMuRate() / yawRate);
88 final double aNight = NIGHT_TURN_LIMIT;
89 final double cNoon = FastMath.cos(aNoon);
90 final double cNight = FastMath.cos(aNight);
91
92 if (context.setUpTurnRegion(cNight, cNoon)) {
93
94 final double absBeta = FastMath.abs(context.getBeta());
95 context.setHalfSpan(context.inSunSide() ?
96 absBeta * FastMath.sqrt(aNoon / absBeta - 1.0) :
97 context.inOrbitPlaneAbsoluteAngle(aNight - FastMath.PI));
98 if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
99
100
101 final double beta = context.getSecuredBeta();
102 final double phiStart = context.getYawStart(beta);
103 final double dtStart = context.timeSinceTurnStart(context.getDate());
104 final double linearPhi;
105 final double phiDot;
106 if (context.inSunSide()) {
107
108 if (beta > 0 && beta < YAW_BIAS) {
109
110
111 phiDot = FastMath.copySign(yawRate, beta);
112 linearPhi = phiStart + phiDot * dtStart;
113 } else {
114
115 phiDot = -FastMath.copySign(yawRate, beta);
116 linearPhi = phiStart + phiDot * dtStart;
117 }
118 } else {
119
120 final double dtEnd = dtStart - context.getTurnDuration();
121 if (dtEnd < 0) {
122
123 phiDot = yawRate;
124 linearPhi = phiStart + phiDot * dtStart;
125 } else {
126
127 phiDot = yawRate;
128 final double phiEnd = phiStart + phiDot * context.getTurnDuration();
129 final double deltaPhi = context.yawAngle() - phiEnd;
130 if (FastMath.abs(deltaPhi / phiDot) <= dtEnd) {
131
132
133 return context.getNominalYaw();
134 } else {
135
136 linearPhi = phiEnd + FastMath.copySign(yawRate * dtEnd, deltaPhi);
137 }
138 }
139 }
140
141 return context.turnCorrectedAttitude(linearPhi, phiDot);
142
143 }
144
145 }
146
147
148 return context.getNominalYaw();
149
150 }
151
152
153 @Override
154 protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {
155
156 final Field<T> field = context.getDate().getField();
157
158
159 final T aNoon = FastMath.atan(context.getMuRate().divide(yawRate));
160 final T aNight = field.getZero().add(NIGHT_TURN_LIMIT);
161 final double cNoon = FastMath.cos(aNoon.getReal());
162 final double cNight = FastMath.cos(aNight.getReal());
163
164 if (context.setUpTurnRegion(cNight, cNoon)) {
165
166 final T absBeta = FastMath.abs(context.getBeta());
167 context.setHalfSpan(context.inSunSide() ?
168 absBeta.multiply(FastMath.sqrt(aNoon.divide(absBeta).subtract(1.0))) :
169 context.inOrbitPlaneAbsoluteAngle(aNight.subtract(FastMath.PI)));
170 if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
171
172
173 final T beta = context.getSecuredBeta();
174 final T phiStart = context.getYawStart(beta);
175 final T dtStart = context.timeSinceTurnStart(context.getDate());
176 final T linearPhi;
177 final T phiDot;
178 if (context.inSunSide()) {
179
180 if (beta.getReal() > 0 && beta.getReal() < YAW_BIAS) {
181
182
183 phiDot = field.getZero().add(FastMath.copySign(yawRate, beta.getReal()));
184 linearPhi = phiStart.add(phiDot.multiply(dtStart));
185 } else {
186
187 phiDot = field.getZero().add(-FastMath.copySign(yawRate, beta.getReal()));
188 linearPhi = phiStart.add(phiDot.multiply(dtStart));
189 }
190 } else {
191
192 final T dtEnd = dtStart.subtract(context.getTurnDuration());
193 if (dtEnd.getReal() < 0) {
194
195 phiDot = field.getZero().add(yawRate);
196 linearPhi = phiStart.add(phiDot.multiply(dtStart));
197 } else {
198
199 phiDot = field.getZero().add(yawRate);
200 final T phiEnd = phiStart.add(phiDot.multiply(context.getTurnDuration()));
201 final T deltaPhi = context.yawAngle().subtract(phiEnd);
202 if (FastMath.abs(deltaPhi.divide(phiDot).getReal()) <= dtEnd.getReal()) {
203
204
205 return context.getNominalYaw();
206 } else {
207
208 linearPhi = phiEnd.add(dtEnd.multiply(yawRate).copySign(deltaPhi));
209 }
210 }
211 }
212
213 return context.turnCorrectedAttitude(linearPhi, phiDot);
214
215 }
216
217 }
218
219
220 return context.getNominalYaw();
221
222 }
223
224 }