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.analysis.differentiation.DerivativeStructure;
20 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
21 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
22 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
23 import org.hipparchus.geometry.euclidean.threed.Vector3D;
24 import org.hipparchus.util.FastMath;
25 import org.orekit.errors.OrekitException;
26 import org.orekit.frames.LOFType;
27 import org.orekit.frames.Transform;
28 import org.orekit.time.AbsoluteDate;
29 import org.orekit.time.TimeStamped;
30 import org.orekit.utils.FieldPVCoordinates;
31 import org.orekit.utils.PVCoordinates;
32 import org.orekit.utils.TimeStampedAngularCoordinates;
33 import org.orekit.utils.TimeStampedPVCoordinates;
34
35 /**
36 * Boilerplate computations for GNSS attitude.
37 *
38 * <p>
39 * This class is intended to hold throw-away data pertaining to <em>one</em> call
40 * to {@link GNSSAttitudeProvider#getAttitude(org.orekit.utils.PVCoordinatesProvider,
41 * org.orekit.time.AbsoluteDate, org.orekit.frames.Frame) getAttitude}. It allows
42 * the various {@link GNSSAttitudeProvider} implementations to be immutable as they
43 * do not store any state, and hence to be thread-safe, reentrant and naturally
44 * serializable (so for example ephemeris built from them are also serializable).
45 * </p>
46 *
47 * @author Luc Maisonobe
48 * @since 9.2
49 */
50 class GNSSAttitudeContext implements TimeStamped {
51
52 /** Constant Y axis. */
53 private static final PVCoordinates PLUS_Y =
54 new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO);
55
56 /** Constant Z axis. */
57 private static final PVCoordinates MINUS_Z =
58 new PVCoordinates(Vector3D.MINUS_K, Vector3D.ZERO, Vector3D.ZERO);
59
60 /** Limit value below which we shoud use replace beta by betaIni. */
61 private static final double BETA_SIGN_CHANGE_PROTECTION = FastMath.toRadians(0.07);
62
63 /** Derivation order. */
64 private static final int ORDER = 2;
65
66 /** Spacecraft position-velocity in inertial frame. */
67 private final TimeStampedPVCoordinates svPV;
68
69 /** Spacecraft position-velocity in inertial frame. */
70 private final FieldPVCoordinates<DerivativeStructure> svPVDS;
71
72 /** Angle between Sun and orbital plane. */
73 private final DerivativeStructure beta;
74
75 /** Cosine of the angle between spacecraft and Sun direction. */
76 private final DerivativeStructure svbCos;
77
78 /** Nominal yaw. */
79 private final TimeStampedAngularCoordinates nominalYaw;
80
81 /** Nominal yaw. */
82 private final FieldRotation<DerivativeStructure> nominalYawDS;
83
84 /** Spacecraft angular velocity. */
85 private double muRate;
86
87 /** Limit cosine for the midnight turn. */
88 private double cNight;
89
90 /** Limit cosine for the noon turn. */
91 private double cNoon;
92
93 /** Relative orbit angle to turn center. */
94 private DerivativeStructure delta;
95
96 /** Half span of the turn region, as an angle in orbit plane. */
97 private double halfSpan;
98
99 /** Turn start date. */
100 private AbsoluteDate turnStart;
101
102 /** Turn end date. */
103 private AbsoluteDate turnEnd;
104
105 /** Simple constructor.
106 * @param sunPV Sun position-velocity in inertial frame
107 * @param svPV spacecraft position-velocity in inertial frame
108 * @exception OrekitException if yaw cannot be corrected
109 */
110 GNSSAttitudeContext(final TimeStampedPVCoordinates sunPV, final TimeStampedPVCoordinates svPV)
111 throws OrekitException {
112
113 final FieldPVCoordinates<DerivativeStructure> sunPVDS = sunPV.toDerivativeStructurePV(ORDER);
114 this.svPV = svPV;
115 this.svPVDS = svPV.toDerivativeStructurePV(ORDER);
116 this.svbCos = FieldVector3D.dotProduct(sunPVDS.getPosition(), svPVDS.getPosition()).
117 divide(sunPVDS.getPosition().getNorm().
118 multiply(svPVDS.getPosition().getNorm()));
119 this.beta = FieldVector3D.angle(sunPVDS.getPosition(), svPVDS.getMomentum()).
120 negate().
121 add(0.5 * FastMath.PI);
122
123 // nominal yaw steering
124 this.nominalYaw =
125 new TimeStampedAngularCoordinates(svPV.getDate(),
126 svPV.normalize(),
127 PVCoordinates.crossProduct(sunPV, svPV).normalize(),
128 MINUS_Z,
129 PLUS_Y,
130 1.0e-9);
131 this.nominalYawDS = nominalYaw.toDerivativeStructureRotation(ORDER);
132
133 this.muRate = svPV.getAngularVelocity().getNorm();
134
135 }
136
137 /** {@inheritDoc} */
138 @Override
139 public AbsoluteDate getDate() {
140 return svPV.getDate();
141 }
142
143 /** Get the cosine of the angle between spacecraft and Sun direction.
144 * @return cosine of the angle between spacecraft and Sun direction.
145 */
146 public double getSVBcos() {
147 return svbCos.getValue();
148 }
149
150 /** Get the angle between Sun and orbital plane.
151 * @return angle between Sun and orbital plane
152 * @see #getBetaDS()
153 * @see #getSecuredBeta(TurnTimeRange)
154 */
155 public double getBeta() {
156 return beta.getValue();
157 }
158
159 /** Get the angle between Sun and orbital plane.
160 * @return angle between Sun and orbital plane
161 * @see #getBeta()
162 * @see #getSecuredBeta(TurnTimeRange)
163 */
164 public DerivativeStructure getBetaDS() {
165 return beta;
166 }
167
168 /** Get a Sun elevation angle that does not change sign within the turn.
169 * <p>
170 * This method either returns the current beta or replaces it with the
171 * value at turn start, so the sign remains constant throughout the
172 * turn. As of 9.2, it is only useful for GPS and Glonass.
173 * </p>
174 * @return secured Sun elevation angle
175 * @see #getBeta()
176 * @see #getBetaDS()
177 */
178 public double getSecuredBeta() {
179 return FastMath.abs(beta.getReal()) < BETA_SIGN_CHANGE_PROTECTION ?
180 beta.taylor(-timeSinceTurnStart(getDate())) :
181 getBeta();
182 }
183
184 /** Get the nominal yaw.
185 * @return nominal yaw
186 */
187 public TimeStampedAngularCoordinates getNominalYaw() {
188 return nominalYaw;
189 }
190
191 /** Compute nominal yaw angle.
192 * @return nominal yaw angle
193 */
194 public double yawAngle() {
195 final Vector3D xSat = nominalYaw.getRotation().revert().applyTo(Vector3D.PLUS_I);
196 return FastMath.copySign(Vector3D.angle(svPV.getVelocity(), xSat), -beta.getReal());
197 }
198
199 /** Compute nominal yaw angle.
200 * @return nominal yaw angle
201 */
202 public DerivativeStructure yawAngleDS() {
203 final FieldVector3D<DerivativeStructure> xSat = nominalYawDS.revert().applyTo(Vector3D.PLUS_I);
204 return FastMath.copySign(FieldVector3D.angle(svPV.getVelocity(), xSat), -beta.getReal());
205 }
206
207 /** Set up the midnight/noon turn region.
208 * @param cosNight limit cosine for the midnight turn
209 * @param cosNoon limit cosine for the noon turn
210 * @return true if spacecraft is in the midnight/noon turn region
211 */
212 public boolean setUpTurnRegion(final double cosNight, final double cosNoon) {
213 this.cNight = cosNight;
214 this.cNoon = cosNoon;
215 if (svbCos.getValue() < cNight) {
216 // in eclipse turn mode
217 final DerivativeStructure absDelta = inOrbitPlaneAbsoluteAngle(svbCos.acos().negate().add(FastMath.PI));
218 delta = FastMath.copySign(absDelta, -absDelta.getPartialDerivative(1));
219 return true;
220 } else if (svbCos.getValue() > cNoon) {
221 // in noon turn mode
222 final DerivativeStructure absDelta = inOrbitPlaneAbsoluteAngle(svbCos.acos());
223 delta = FastMath.copySign(absDelta, -absDelta.getPartialDerivative(1));
224 return true;
225 } else {
226 return false;
227 }
228 }
229
230 /** Get the relative orbit angle to turn center.
231 * @return relative orbit angle to turn center
232 */
233 public double getDelta() {
234 return delta.getValue();
235 }
236
237 /** Get the relative orbit angle to turn center.
238 * @return relative orbit angle to turn center
239 */
240 public DerivativeStructure getDeltaDS() {
241 return delta;
242 }
243
244 /** Check if spacecraft is in the half orbit closest to Sun.
245 * @return true if spacecraft is in the half orbit closest to Sun
246 */
247 public boolean inSunSide() {
248 return svbCos.getValue() > 0;
249 }
250
251 /** Get yaw at turn start.
252 * @param sunBeta Sun elevation above orbital plane
253 * (it <em>may</em> be different from {@link #getBeta()} in
254 * some special cases)
255 * @return yaw at turn start
256 */
257 public double getYawStart(final double sunBeta) {
258 return computePhi(sunBeta, FastMath.copySign(halfSpan, svbCos.getValue()));
259 }
260
261 /** Get yaw at turn end.
262 * @param sunBeta Sun elevation above orbital plane
263 * (it <em>may</em> be different from {@link #getBeta()} in
264 * some special cases)
265 * @return yaw at turn end
266 */
267 public double getYawEnd(final double sunBeta) {
268 return computePhi(sunBeta, FastMath.copySign(halfSpan, -svbCos.getValue()));
269 }
270
271 /** Compute yaw rate.
272 * @param sunBeta Sun elevation above orbital plane
273 * (it <em>may</em> be different from {@link #getBeta()} in
274 * some special cases)
275 * @return yaw rate
276 */
277 public double yawRate(final double sunBeta) {
278 return (getYawEnd(sunBeta) - getYawStart(sunBeta)) / getTurnDuration();
279 }
280
281 /** Get the spacecraft angular velocity.
282 * @return spacecraft angular velocity
283 */
284 public double getMuRate() {
285 return muRate;
286 }
287
288 /** Project a spacecraft/Sun angle into orbital plane.
289 * <p>
290 * This method is intended to find the limits of the noon and midnight
291 * turns in orbital plane. The return angle is signed, depending on the
292 * spacecraft being before or after turn middle point.
293 * </p>
294 * @param angle spacecraft/Sun angle (or spacecraft/opposite-of-Sun)
295 * @return angle projected into orbital plane, always positive
296 */
297 private DerivativeStructure inOrbitPlaneAbsoluteAngle(final DerivativeStructure angle) {
298 return FastMath.acos(FastMath.cos(angle).divide(FastMath.cos(beta)));
299 }
300
301 /** Project a spacecraft/Sun angle into orbital plane.
302 * <p>
303 * This method is intended to find the limits of the noon and midnight
304 * turns in orbital plane. The return angle is always positive. The
305 * correct sign to apply depends on the spacecraft being before or
306 * after turn middle point.
307 * </p>
308 * @param angle spacecraft/Sun angle (or spacecraft/opposite-of-Sun)
309 * @return angle projected into orbital plane, always positive
310 */
311 public double inOrbitPlaneAbsoluteAngle(final double angle) {
312 return FastMath.acos(FastMath.cos(angle) / FastMath.cos(beta.getReal()));
313 }
314
315 /** Compute yaw.
316 * @param sunBeta Sun elevation above orbital plane
317 * (it <em>may</em> be different from {@link #getBeta()} in
318 * some special cases)
319 * @param inOrbitPlaneAngle in orbit angle between spacecraft
320 * and Sun (or opposite of Sun) projection
321 * @return yaw angle
322 */
323 public double computePhi(final double sunBeta, final double inOrbitPlaneAngle) {
324 return FastMath.atan2(-FastMath.tan(sunBeta), FastMath.sin(inOrbitPlaneAngle));
325 }
326
327 /** Set turn half span and compute corresponding turn time range.
328 * @param halfSpan half span of the turn region, as an angle in orbit plane
329 */
330 public void setHalfSpan(final double halfSpan) {
331 this.halfSpan = halfSpan;
332 this.turnStart = svPV.getDate().shiftedBy((delta.getValue() - halfSpan) / muRate);
333 this.turnEnd = svPV.getDate().shiftedBy((delta.getValue() + halfSpan) / muRate);
334 }
335
336 /** Check if a date is within range.
337 * @param date date to check
338 * @param endMargin margin in seconds after turn end
339 * @return true if date is within range extended by end margin
340 */
341 public boolean inTurnTimeRange(final AbsoluteDate date, final double endMargin) {
342 return date.durationFrom(turnStart) > 0 &&
343 date.durationFrom(turnEnd) < endMargin;
344 }
345
346 /** Get turn duration.
347 * @return turn duration
348 */
349 public double getTurnDuration() {
350 return 2 * halfSpan / muRate;
351 }
352
353 /** Get elapsed time since turn start.
354 * @param date date to check
355 * @return elapsed time from turn start to specified date
356 */
357 public double timeSinceTurnStart(final AbsoluteDate date) {
358 return date.durationFrom(turnStart);
359 }
360
361 /** Generate an attitude with turn-corrected yaw.
362 * @param yaw yaw value to apply
363 * @param yawDot yaw first time derivative
364 * @return attitude with specified yaw
365 */
366 public TimeStampedAngularCoordinates turnCorrectedAttitude(final double yaw, final double yawDot) {
367 return turnCorrectedAttitude(beta.getFactory().build(yaw, yawDot, 0.0));
368 }
369
370 /** Generate an attitude with turn-corrected yaw.
371 * @param yaw yaw value to apply
372 * @return attitude with specified yaw
373 */
374 public TimeStampedAngularCoordinates turnCorrectedAttitude(final DerivativeStructure yaw) {
375
376 // compute a linear yaw correction model
377 final DerivativeStructure nominalAngle = yawAngleDS();
378 final TimeStampedAngularCoordinates correction =
379 new TimeStampedAngularCoordinates(nominalYaw.getDate(),
380 new FieldRotation<>(FieldVector3D.getPlusK(nominalAngle.getField()),
381 yaw.subtract(nominalAngle),
382 RotationConvention.VECTOR_OPERATOR));
383
384 // combine the two parts of the attitude
385 return correction.addOffset(getNominalYaw());
386
387 }
388
389 /** Compute Orbit Normal (ON) yaw.
390 * @return Orbit Normal yaw, using inertial frame as reference
391 */
392 public TimeStampedAngularCoordinates orbitNormalYaw() {
393 final Transform t = LOFType.VVLH.transformFromInertial(svPV.getDate(), svPV);
394 return new TimeStampedAngularCoordinates(svPV.getDate(),
395 t.getRotation(),
396 t.getRotationRate(),
397 t.getRotationAcceleration());
398 }
399
400 }