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.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
22 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
23 import org.hipparchus.util.FastMath;
24 import org.hipparchus.util.FieldSinCos;
25 import org.orekit.frames.Frame;
26 import org.orekit.time.AbsoluteDate;
27 import org.orekit.utils.ExtendedPositionProvider;
28 import org.orekit.utils.TimeStampedAngularCoordinates;
29 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44 public class Galileo extends AbstractGNSSAttitudeProvider {
45
46
47 public static final double DEFAULT_YAW_RATE = FastMath.toRadians(0.203);
48
49
50 private static final double BETA_X = FastMath.toRadians(15.0);
51
52
53 private static final double COS_NOON = FastMath.cos(BETA_X);
54
55
56 private static final double COS_NIGHT = -COS_NOON;
57
58
59 private static final double END_MARGIN = 0.0;
60
61
62 private final double yawRate;
63
64
65
66
67
68
69
70
71 public Galileo(final double yawRate,
72 final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
73 final ExtendedPositionProvider sun, final Frame inertialFrame) {
74 super(validityStart, validityEnd, sun, inertialFrame);
75 this.yawRate = yawRate;
76 }
77
78
79 @Override
80 protected TimeStampedAngularCoordinates correctedYaw(final GNSSAttitudeContext context) {
81
82
83 final double beta0 = FastMath.atan(context.getMuRate() / yawRate);
84
85 if (FastMath.abs(context.betaD2().getValue()) < beta0 &&
86 context.setUpTurnRegion(COS_NIGHT, COS_NOON)) {
87
88 context.setHalfSpan(context.inSunSide() ?
89 BETA_X :
90 context.inOrbitPlaneAbsoluteAngle(BETA_X),
91 END_MARGIN);
92 if (context.inTurnTimeRange()) {
93
94
95 final UnivariateDerivative2 beta = context.betaD2();
96 final FieldSinCos<UnivariateDerivative2> scBeta = FastMath.sinCos(beta);
97 final UnivariateDerivative2 cosBeta = scBeta.cos();
98 final UnivariateDerivative2 sinBeta = scBeta.sin();
99 final double sinY = FastMath.copySign(FastMath.sin(beta0), context.getSecuredBeta());
100 final UnivariateDerivative2 sd = FastMath.sin(context.getDeltaDS()).
101 multiply(FastMath.copySign(1.0, -context.getSVBcos() * context.getDeltaDS().getPartialDerivative(1)));
102 final UnivariateDerivative2 c = sd.multiply(cosBeta);
103 final UnivariateDerivative2 shy = sinBeta.negate().subtract(sinY).
104 add(sinBeta.subtract(sinY).multiply(c.abs().multiply(c.getPi().divide(FastMath.sin(BETA_X))).cos())).
105 multiply(0.5);
106 final UnivariateDerivative2 phi = FastMath.atan2(shy, c);
107
108 return context.turnCorrectedAttitude(phi);
109
110 }
111
112 }
113
114
115 return context.nominalYaw(context.getDate());
116
117 }
118
119
120 @Override
121 protected <T extends CalculusFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {
122
123
124 final double beta0 = FastMath.atan(context.getMuRate().getReal() / yawRate);
125
126 if (FastMath.abs(context.beta(context.getDate())).getReal() < beta0 &&
127 context.setUpTurnRegion(COS_NIGHT, COS_NOON)) {
128
129 final Field<T> field = context.getDate().getField();
130 final T betaX = field.getZero().newInstance(BETA_X);
131 context.setHalfSpan(context.inSunSide() ?
132 betaX :
133 context.inOrbitPlaneAbsoluteAngle(betaX),
134 END_MARGIN);
135 if (context.inTurnTimeRange()) {
136
137
138 final FieldUnivariateDerivative2<T> beta = context.betaD2();
139 final FieldSinCos<FieldUnivariateDerivative2<T>> scBeta = FastMath.sinCos(beta);
140 final FieldUnivariateDerivative2<T> cosBeta = scBeta.cos();
141 final FieldUnivariateDerivative2<T> sinBeta = scBeta.sin();
142 final T sinY = FastMath.sin(field.getZero().add(beta0)).copySign(context.getSecuredBeta());
143 final FieldUnivariateDerivative2<T> sd = FastMath.sin(context.getDeltaDS()).
144 multiply(FastMath.copySign(1.0, -context.getSVBcos().getReal() * context.getDeltaDS().getPartialDerivative(1).getReal()));
145 final FieldUnivariateDerivative2<T> c = sd.multiply(cosBeta);
146 final FieldUnivariateDerivative2<T> shy = sinBeta.negate().subtract(sinY).
147 add(sinBeta.subtract(sinY).multiply(c.abs().multiply(c.getPi().divide(FastMath.sin(BETA_X))).cos())).
148 multiply(0.5);
149 final FieldUnivariateDerivative2<T> phi = FastMath.atan2(shy, c);
150
151 return context.turnCorrectedAttitude(phi);
152
153 }
154
155 }
156
157
158 return context.nominalYaw(context.getDate());
159
160 }
161
162 }