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 public class GPSBlockIIF extends AbstractGNSSAttitudeProvider {
42
43
44 public static final double DEFAULT_YAW_RATE = FastMath.toRadians(0.11);
45
46
47 public static final double DEFAULT_YAW_BIAS = FastMath.toRadians(-0.7);
48
49
50 private static final long serialVersionUID = 20171114L;
51
52
53 private static final double NIGHT_TURN_LIMIT = FastMath.toRadians(180.0 - 13.25);
54
55
56 private final double END_MARGIN = 1800.0;
57
58
59 private final double yawRate;
60
61
62 private final double yawBias;
63
64
65
66
67
68
69
70
71
72 public GPSBlockIIF(final double yawRate, final double yawBias,
73 final AbsoluteDateDate">AbsoluteDate validityStart, final AbsoluteDate validityEnd,
74 final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
75 super(validityStart, validityEnd, sun, inertialFrame);
76 this.yawRate = yawRate;
77 this.yawBias = yawBias;
78 }
79
80
81 @Override
82 protected TimeStampedAngularCoordinates correctedYaw(final GNSSAttitudeContext context) {
83
84
85 final double aNoon = FastMath.atan(context.getMuRate() / yawRate);
86 final double aNight = NIGHT_TURN_LIMIT;
87 final double cNoon = FastMath.cos(aNoon);
88 final double cNight = FastMath.cos(aNight);
89
90 if (context.setUpTurnRegion(cNight, cNoon)) {
91
92 final double absBeta = FastMath.abs(context.beta(context.getDate()));
93 context.setHalfSpan(context.inSunSide() ?
94 absBeta * FastMath.sqrt(aNoon / absBeta - 1.0) :
95 context.inOrbitPlaneAbsoluteAngle(aNight - FastMath.PI),
96 END_MARGIN);
97 if (context.inTurnTimeRange()) {
98
99
100 final double beta = context.getSecuredBeta();
101 final double phiStart = context.getYawStart(beta);
102 final double dtStart = context.timeSinceTurnStart();
103 final double phiDot;
104 final double linearPhi;
105 if (context.inSunSide()) {
106
107 if (beta > yawBias && beta < 0) {
108
109
110 phiDot = FastMath.copySign(yawRate, beta);
111 linearPhi = phiStart + phiDot * dtStart;
112 } else {
113
114 phiDot = -FastMath.copySign(yawRate, beta);
115 linearPhi = phiStart + phiDot * dtStart;
116 }
117 } else {
118
119 phiDot = context.yawRate(beta);
120 linearPhi = phiStart + phiDot * dtStart;
121 }
122
123 if (context.linearModelStillActive(linearPhi, phiDot)) {
124
125 return context.turnCorrectedAttitude(linearPhi, phiDot);
126 }
127
128
129 }
130
131 }
132
133
134 return context.nominalYaw(context.getDate());
135
136 }
137
138
139 @Override
140 protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {
141
142 final Field<T> field = context.getDate().getField();
143
144
145 final T aNoon = FastMath.atan(context.getMuRate().divide(yawRate));
146 final T aNight = field.getZero().add(NIGHT_TURN_LIMIT);
147 final double cNoon = FastMath.cos(aNoon.getReal());
148 final double cNight = FastMath.cos(aNight.getReal());
149
150 if (context.setUpTurnRegion(cNight, cNoon)) {
151
152 final T absBeta = FastMath.abs(context.beta(context.getDate()));
153 context.setHalfSpan(context.inSunSide() ?
154 absBeta.multiply(FastMath.sqrt(aNoon.divide(absBeta).subtract(1.0))) :
155 context.inOrbitPlaneAbsoluteAngle(aNight.subtract(FastMath.PI)),
156 END_MARGIN);
157 if (context.inTurnTimeRange()) {
158
159
160 final T beta = context.getSecuredBeta();
161 final T phiStart = context.getYawStart(beta);
162 final T dtStart = context.timeSinceTurnStart();
163 final T phiDot;
164 final T linearPhi;
165 if (context.inSunSide()) {
166
167 if (beta.getReal() > yawBias && beta.getReal() < 0) {
168
169
170 phiDot = field.getZero().add(FastMath.copySign(yawRate, beta.getReal()));
171 linearPhi = phiStart.add(phiDot.multiply(dtStart));
172 } else {
173
174 phiDot = field.getZero().add(-FastMath.copySign(yawRate, beta.getReal()));
175 linearPhi = phiStart.add(phiDot.multiply(dtStart));
176 }
177 } else {
178
179 phiDot = context.yawRate(beta);
180 linearPhi = phiStart.add(phiDot.multiply(dtStart));
181 }
182
183 if (context.linearModelStillActive(linearPhi, phiDot)) {
184
185 return context.turnCorrectedAttitude(linearPhi, phiDot);
186 }
187
188 }
189
190 }
191
192
193 return context.nominalYaw(context.getDate());
194
195 }
196
197 }