Commit 4296a0b0 authored by Luc Maisonobe's avatar Luc Maisonobe

Added field versions of the GNSS attitude modes.

parent c4112999
......@@ -23,9 +23,12 @@ import org.orekit.errors.OrekitException;
import org.orekit.frames.Frame;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
import org.orekit.utils.FieldPVCoordinatesProvider;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;
import org.orekit.utils.TimeStampedFieldPVCoordinates;
import org.orekit.utils.TimeStampedPVCoordinates;
/**
......@@ -46,7 +49,7 @@ public abstract class AbstractGNSSAttitudeProvider implements GNSSAttitudeProvid
private final AbsoluteDate validityEnd;
/** Provider for Sun position. */
private final PVCoordinatesProvider sun;
private final ExtendedPVCoordinatesProvider sun;
/** Inertial frame where velocity are computed. */
private final Frame inertialFrame;
......@@ -59,7 +62,7 @@ public abstract class AbstractGNSSAttitudeProvider implements GNSSAttitudeProvid
*/
protected AbstractGNSSAttitudeProvider(final AbsoluteDate validityStart,
final AbsoluteDate validityEnd,
final PVCoordinatesProvider sun,
final ExtendedPVCoordinatesProvider sun,
final Frame inertialFrame) {
this.validityStart = validityStart;
this.validityEnd = validityEnd;
......@@ -104,8 +107,17 @@ public abstract class AbstractGNSSAttitudeProvider implements GNSSAttitudeProvid
final FieldAbsoluteDate<T> date,
final Frame frame)
throws OrekitException {
// TODO
return null;
// Sun/spacecraft geometry
// computed in inertial frame so orbital plane (which depends on spacecraft velocity) is correct
final TimeStampedFieldPVCoordinates<T> sunPV = sun.getPVCoordinates(date, inertialFrame);
final TimeStampedFieldPVCoordinates<T> svPV = pvProv.getPVCoordinates(date, inertialFrame);
// compute yaw correction
final TimeStampedFieldAngularCoordinates<T> corrected = correctedYaw(new GNSSFieldAttitudeContext<>(sunPV, svPV));
return new FieldAttitude<>(inertialFrame, corrected).withReferenceFrame(frame);
}
/** Compute GNSS attitude with midnight/noon yaw turn correction.
......@@ -116,4 +128,14 @@ public abstract class AbstractGNSSAttitudeProvider implements GNSSAttitudeProvid
protected abstract TimeStampedAngularCoordinates correctedYaw(GNSSAttitudeContext context)
throws OrekitException;
/** Compute GNSS attitude with midnight/noon yaw turn correction.
* @param context context data for attitude computation
* @param <T> type of the field elements
* @return corrected yaw, using inertial frame as the reference
* @exception OrekitException if yaw corrected attitude cannot be computed
*/
protected abstract <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T>
correctedYaw(GNSSFieldAttitudeContext<T> context)
throws OrekitException;
}
......@@ -16,11 +16,13 @@
*/
package org.orekit.gnss.attitude;
import org.hipparchus.RealFieldElement;
import org.orekit.errors.OrekitException;
import org.orekit.frames.Frame;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;
/**
* Attitude providers for Beidou geostationary orbit navigation satellites.
......@@ -39,7 +41,7 @@ public class BeidouGeo extends AbstractGNSSAttitudeProvider {
* @param inertialFrame inertial frame where velocity are computed
*/
public BeidouGeo(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
final PVCoordinatesProvider sun, final Frame inertialFrame) {
final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
super(validityStart, validityEnd, sun, inertialFrame);
}
......@@ -53,4 +55,14 @@ public class BeidouGeo extends AbstractGNSSAttitudeProvider {
}
/** {@inheritDoc} */
@Override
protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context)
throws OrekitException {
// geostationary Beidou spacecraft are always in Orbit Normal (ON) yaw
return context.orbitNormalYaw();
}
}
......@@ -16,12 +16,14 @@
*/
package org.orekit.gnss.attitude;
import org.hipparchus.RealFieldElement;
import org.hipparchus.util.FastMath;
import org.orekit.errors.OrekitException;
import org.orekit.frames.Frame;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;
/**
* Attitude providers for Beidou inclined geosynchronous orbit navigation satellites.
......@@ -43,7 +45,7 @@ public class BeidouIGSO extends AbstractGNSSAttitudeProvider {
* @param inertialFrame inertial frame where velocity are computed
*/
public BeidouIGSO(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
final PVCoordinatesProvider sun, final Frame inertialFrame) {
final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
super(validityStart, validityEnd, sun, inertialFrame);
}
......@@ -62,4 +64,19 @@ public class BeidouIGSO extends AbstractGNSSAttitudeProvider {
}
/** {@inheritDoc} */
@Override
protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context)
throws OrekitException {
if (FastMath.abs(context.getBeta()).getReal() < 2 * BETA_0) {
// when Sun is close to orbital plane, attitude is in Orbit Normal (ON) yaw
return context.orbitNormalYaw();
}
// in nominal yaw mode
return context.getNominalYaw();
}
}
......@@ -16,12 +16,14 @@
*/
package org.orekit.gnss.attitude;
import org.hipparchus.RealFieldElement;
import org.hipparchus.util.FastMath;
import org.orekit.errors.OrekitException;
import org.orekit.frames.Frame;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;
/**
* Attitude providers for Beidou Medium Earth Orbit navigation satellites.
......@@ -43,7 +45,7 @@ public class BeidouMeo extends AbstractGNSSAttitudeProvider {
* @param inertialFrame inertial frame where velocity are computed
*/
public BeidouMeo(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
final PVCoordinatesProvider sun, final Frame inertialFrame) {
final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
super(validityStart, validityEnd, sun, inertialFrame);
}
......@@ -62,4 +64,19 @@ public class BeidouMeo extends AbstractGNSSAttitudeProvider {
}
/** {@inheritDoc} */
@Override
protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context)
throws OrekitException {
if (FastMath.abs(context.getBeta()).getReal() < 2 * BETA_0) {
// when Sun is close to orbital plane, attitude is in Orbit Normal (ON) yaw
return context.orbitNormalYaw();
}
// in nominal yaw mode
return context.getNominalYaw();
}
}
......@@ -16,11 +16,14 @@
*/
package org.orekit.gnss.attitude;
import org.hipparchus.Field;
import org.hipparchus.RealFieldElement;
import org.hipparchus.util.FastMath;
import org.orekit.frames.Frame;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;
/**
* Attitude providers for GPS block IIR navigation satellites.
......@@ -68,7 +71,7 @@ public class GPSBlockIIA extends AbstractGNSSAttitudeProvider {
* @param prnNumber number within the GPS constellation (between 1 and 32)
*/
public GPSBlockIIA(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
final PVCoordinatesProvider sun, final Frame inertialFrame, final int prnNumber) {
final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame, final int prnNumber) {
super(validityStart, validityEnd, sun, inertialFrame);
yawRate = FastMath.toRadians(YAW_RATES[prnNumber - 1]);
}
......@@ -143,4 +146,76 @@ public class GPSBlockIIA extends AbstractGNSSAttitudeProvider {
}
/** {@inheritDoc} */
@Override
protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {
final Field<T> field = context.getDate().getField();
// noon beta angle limit from yaw rate
final T aNoon = FastMath.atan(context.getMuRate().divide(yawRate));
final T aNight = field.getZero().add(NIGHT_TURN_LIMIT);
final double cNoon = FastMath.cos(aNoon.getReal());
final double cNight = FastMath.cos(aNight.getReal());
if (context.setUpTurnRegion(cNight, cNoon)) {
final T absBeta = FastMath.abs(context.getBeta());
context.setHalfSpan(context.inSunSide() ?
absBeta.multiply(FastMath.sqrt(aNoon.divide(absBeta).subtract(1.0))) :
context.inOrbitPlaneAbsoluteAngle(aNight.subtract(FastMath.PI)));
if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
// we need to ensure beta sign does not change during the turn
final T beta = context.getSecuredBeta();
final T phiStart = context.getYawStart(beta);
final T dtStart = context.timeSinceTurnStart(context.getDate());
final T linearPhi;
final T phiDot;
if (context.inSunSide()) {
// noon turn
if (beta.getReal() > 0 && beta.getReal() < YAW_BIAS) {
// noon turn problem for small positive beta in block IIA
// rotation is in the wrong direction for these spacecrafts
phiDot = field.getZero().add(FastMath.copySign(yawRate, beta.getReal()));
linearPhi = phiStart.add(phiDot.multiply(dtStart));
} else {
// regular noon turn
phiDot = field.getZero().add(-FastMath.copySign(yawRate, beta.getReal()));
linearPhi = phiStart.add(phiDot.multiply(dtStart));
}
} else {
// midnight turn
final T dtEnd = dtStart.subtract(context.getTurnDuration());
if (dtEnd.getReal() < 0) {
// we are within the turn itself
phiDot = field.getZero().add(yawRate);
linearPhi = phiStart.add(phiDot.multiply(dtStart));
} else {
// we are in the recovery phase after turn
phiDot = field.getZero().add(yawRate);
final T phiEnd = phiStart.add(phiDot.multiply(context.getTurnDuration()));
final T deltaPhi = context.yawAngle().subtract(phiEnd);
if (FastMath.abs(deltaPhi.divide(phiDot).getReal()) <= dtEnd.getReal()) {
// time since turn end was sufficient for recovery
// we are already back in nominal yaw mode
return context.getNominalYaw();
} else {
// recovery is not finished yet
linearPhi = phiEnd.add(dtEnd.multiply(yawRate).copySign(deltaPhi));
}
}
}
return context.turnCorrectedAttitude(linearPhi, phiDot);
}
}
// in nominal yaw mode
return context.getNominalYaw();
}
}
......@@ -16,11 +16,14 @@
*/
package org.orekit.gnss.attitude;
import org.hipparchus.Field;
import org.hipparchus.RealFieldElement;
import org.hipparchus.util.FastMath;
import org.orekit.frames.Frame;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;
/**
* Attitude providers for GPS block IIF navigation satellites.
......@@ -59,7 +62,7 @@ public class GPSBlockIIF extends AbstractGNSSAttitudeProvider {
* @param inertialFrame inertial frame where velocity are computed
*/
public GPSBlockIIF(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
final PVCoordinatesProvider sun, final Frame inertialFrame) {
final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
super(validityStart, validityEnd, sun, inertialFrame);
}
......@@ -116,4 +119,59 @@ public class GPSBlockIIF extends AbstractGNSSAttitudeProvider {
}
/** {@inheritDoc} */
@Override
protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {
final Field<T> field = context.getDate().getField();
// noon beta angle limit from yaw rate
final T aNoon = FastMath.atan(context.getMuRate().divide(YAW_RATE));
final T aNight = field.getZero().add(NIGHT_TURN_LIMIT);
final double cNoon = FastMath.cos(aNoon.getReal());
final double cNight = FastMath.cos(aNight.getReal());
if (context.setUpTurnRegion(cNight, cNoon)) {
final T absBeta = FastMath.abs(context.getBeta());
context.setHalfSpan(context.inSunSide() ?
absBeta.multiply(FastMath.sqrt(aNoon.divide(absBeta).subtract(1.0))) :
context.inOrbitPlaneAbsoluteAngle(aNight.subtract(FastMath.PI)));
if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
// we need to ensure beta sign does not change during the turn
final T beta = context.getSecuredBeta();
final T phiStart = context.getYawStart(beta);
final T dtStart = context.timeSinceTurnStart(context.getDate());
final T phiDot;
final T linearPhi;
if (context.inSunSide()) {
// noon turn
if (beta.getReal() > YAW_BIAS && beta.getReal() < 0) {
// noon turn problem for small negative beta in block IIF
// rotation is in the wrong direction for these spacecrafts
phiDot = field.getZero().add(FastMath.copySign(YAW_RATE, beta.getReal()));
linearPhi = phiStart.add(phiDot.multiply(dtStart));
} else {
// regular noon turn
phiDot = field.getZero().add(-FastMath.copySign(YAW_RATE, beta.getReal()));
linearPhi = phiStart.add(phiDot.multiply(dtStart));
}
} else {
// midnight turn
phiDot = context.yawRate(beta);
linearPhi = phiStart.add(phiDot.multiply(dtStart));
}
return context.turnCorrectedAttitude(linearPhi, phiDot);
}
}
// in nominal yaw mode
return context.getNominalYaw();
}
}
......@@ -16,11 +16,14 @@
*/
package org.orekit.gnss.attitude;
import org.hipparchus.Field;
import org.hipparchus.RealFieldElement;
import org.hipparchus.util.FastMath;
import org.orekit.frames.Frame;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;
/**
* Attitude providers for GPS block IIF navigation satellites.
......@@ -53,7 +56,7 @@ public class GPSBlockIIR extends AbstractGNSSAttitudeProvider {
* @param inertialFrame inertial frame where velocity are computed
*/
public GPSBlockIIR(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
final PVCoordinatesProvider sun, final Frame inertialFrame) {
final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
super(validityStart, validityEnd, sun, inertialFrame);
}
......@@ -104,4 +107,53 @@ public class GPSBlockIIR extends AbstractGNSSAttitudeProvider {
}
/** {@inheritDoc} */
@Override
protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {
final Field<T> field = context.getDate().getField();
// noon beta angle limit from yaw rate
final T aNoon = FastMath.atan(context.getMuRate().divide(YAW_RATE));
final double cNoon = FastMath.cos(aNoon.getReal());
final double cNight = -cNoon;
if (context.setUpTurnRegion(cNight, cNoon)) {
final T absBeta = FastMath.abs(context.getBeta());
context.setHalfSpan(absBeta.multiply(FastMath.sqrt(aNoon.divide(absBeta).subtract(1.0))));
if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
// we need to ensure beta sign does not change during the turn
final T beta = context.getSecuredBeta();
final T phiStart = context.getYawStart(beta);
final T dtStart = context.timeSinceTurnStart(context.getDate());
final T phiDot;
final T linearPhi;
if (context.inSunSide()) {
// noon turn
phiDot = field.getZero().add(-FastMath.copySign(YAW_RATE, beta.getReal()));
linearPhi = phiStart.add(phiDot.multiply(dtStart));
} else {
// midnight turn
phiDot = field.getZero().add(FastMath.copySign(YAW_RATE, beta.getReal()));
linearPhi = phiStart.add(phiDot.multiply(dtStart));
final T phiEnd = context.getYawEnd(beta);
if (phiEnd.getReal() / linearPhi.getReal() < 0 || phiEnd.getReal() / linearPhi.getReal() > 1) {
return context.getNominalYaw();
}
}
return context.turnCorrectedAttitude(linearPhi, phiDot);
}
}
// in nominal yaw mode
return context.getNominalYaw();
}
}
......@@ -16,12 +16,16 @@
*/
package org.orekit.gnss.attitude;
import org.hipparchus.Field;
import org.hipparchus.RealFieldElement;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.analysis.differentiation.FieldDerivativeStructure;
import org.hipparchus.util.FastMath;
import org.orekit.frames.Frame;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;
/**
* Attitude providers for Galileo navigation satellites.
......@@ -63,7 +67,7 @@ public class Galileo extends AbstractGNSSAttitudeProvider {
* @param inertialFrame inertial frame where velocity are computed
*/
public Galileo(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
final PVCoordinatesProvider sun, final Frame inertialFrame) {
final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
super(validityStart, validityEnd, sun, inertialFrame);
}
......@@ -103,4 +107,42 @@ public class Galileo extends AbstractGNSSAttitudeProvider {
}
/** {@inheritDoc} */
@Override
protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {
if (FastMath.abs(context.getBeta()).getReal() < BETA_Y &&
context.setUpTurnRegion(COS_NIGHT, COS_NOON)) {
final Field<T> field = context.getDate().getField();
final T betaX = field.getZero().add(BETA_X);
context.setHalfSpan(context.inSunSide() ?
betaX :
context.inOrbitPlaneAbsoluteAngle(betaX));
if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
// handling both noon and midnight turns at once
final FieldDerivativeStructure<T> beta = context.getBetaDS();
final FieldDerivativeStructure<T> cosBeta = beta.cos();
final FieldDerivativeStructure<T> sinBeta = beta.sin();
final T sinY = FastMath.sin(field.getZero().add(BETA_Y)).copySign(context.getSecuredBeta());
final FieldDerivativeStructure<T> sd = FastMath.sin(context.getDeltaDS()).
multiply(FastMath.copySign(1.0, -context.getSVBcos().getReal() * context.getDeltaDS().getPartialDerivative(1).getReal()));
final FieldDerivativeStructure<T> c = sd.multiply(cosBeta);
final FieldDerivativeStructure<T> shy = sinBeta.negate().subtract(sinY).
add(sinBeta.subtract(sinY).multiply(c.abs().multiply(FastMath.PI / FastMath.sin(BETA_X)).cos())).
multiply(0.5);
final FieldDerivativeStructure<T> phi = FastMath.atan2(shy, c);
return context.turnCorrectedAttitude(phi);
}
}
// in nominal yaw mode
return context.getNominalYaw();
}
}
......@@ -16,10 +16,12 @@
*/
package org.orekit.gnss.attitude;
import org.hipparchus.RealFieldElement;
import org.orekit.frames.Frame;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;
/**
* Attitude providers for navigation satellites for which no specialized model is known.
......@@ -39,7 +41,7 @@ public class GenericGNSS extends AbstractGNSSAttitudeProvider {
* @param inertialFrame inertial frame where velocity are computed
*/
public GenericGNSS(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
final PVCoordinatesProvider sun, final Frame inertialFrame) {
final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
super(validityStart, validityEnd, sun, inertialFrame);
}
......@@ -50,4 +52,11 @@ public class GenericGNSS extends AbstractGNSSAttitudeProvider {
return context.getNominalYaw();
}
/** {@inheritDoc} */
@Override
protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context) {
// no eclipse/noon turn mode for generic spacecraft
return context.getNominalYaw();
}
}
......@@ -16,12 +16,15 @@
*/
package org.orekit.gnss.attitude;
import org.hipparchus.Field;
import org.hipparchus.RealFieldElement;
import org.hipparchus.util.FastMath;
import org.orekit.errors.OrekitException;
import org.orekit.frames.Frame;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
import org.orekit.utils.TimeStampedFieldAngularCoordinates;
/**
* Attitude providers for Glonass navigation satellites.
......@@ -57,7 +60,7 @@ public class Glonass extends AbstractGNSSAttitudeProvider {
* @param inertialFrame inertial frame where velocity are computed
*/
public Glonass(final AbsoluteDate validityStart, final AbsoluteDate validityEnd,
final PVCoordinatesProvider sun, final Frame inertialFrame) {
final ExtendedPVCoordinatesProvider sun, final Frame inertialFrame) {
super(validityStart, validityEnd, sun, inertialFrame);
}
......@@ -123,4 +126,69 @@ public class Glonass extends AbstractGNSSAttitudeProvider {
}
/** {@inheritDoc} */
@Override
protected <T extends RealFieldElement<T>> TimeStampedFieldAngularCoordinates<T> correctedYaw(final GNSSFieldAttitudeContext<T> context)
throws OrekitException {
final Field<T> field = context.getDate().getField();
// noon beta angle limit from yaw rate
final T realBeta = context.getBeta();
final T muRate = context.getMuRate();
final T aNight = field.getZero().add(NIGHT_TURN_LIMIT);
T aNoon = FastMath.atan(muRate.divide(YAW_RATE));
if (FastMath.abs(realBeta).getReal() < aNoon.getReal()) {
T yawEnd = field.getZero().add(YAW_END_ZERO);
for (int i = 0; i < 3; ++i) {
final T delta = muRate.multiply(yawEnd).divide(YAW_RATE);
yawEnd = FastMath.abs(context.computePhi(realBeta, delta).
subtract(context.computePhi(realBeta, delta.negate()))).
multiply(0.5);
}
aNoon = muRate.multiply(yawEnd).divide(YAW_RATE);
}
final double cNoon = FastMath.cos(aNoon.getReal());
final double cNight = FastMath.cos(aNight.getReal());
if (context.setUpTurnRegion(cNight, cNoon)) {
context.setHalfSpan(context.inSunSide() ?
aNoon :
context.inOrbitPlaneAbsoluteAngle(aNight.subtract(FastMath.PI)));
if (context.inTurnTimeRange(context.getDate(), 0)) {
// we need to ensure beta sign does not change during the turn
final T beta = context.getSecuredBeta();
final T phiStart = context.getYawStart(beta);
final T dtStart = context.timeSinceTurnStart(context.getDate());
final T phiDot;
final T linearPhi;
final T phiEnd = context.getYawEnd(beta);
if (context.inSunSide()) {
// noon turn
phiDot = field.getZero().add(-FastMath.copySign(YAW_RATE, beta.getReal()));
linearPhi = phiStart.add(phiDot.multiply(dtStart));
} else {
// midnight turn
phiDot = field.getZero().add(FastMath.copySign(YAW_RATE, beta.getReal()));
linearPhi = phiStart.add(phiDot.multiply(dtStart));
}
if (phiEnd.getReal() / linearPhi.getReal() < 0 || phiEnd.getReal() / linearPhi.getReal() > 1) {
return context.turnCorrectedAttitude(phiEnd, field.getZero());
} else {
return context.turnCorrectedAttitude(linearPhi, phiDot);
}
}
}
// in nominal yaw mode
return context.getNominalYaw();
}
}
......@@ -168,6 +168,7 @@ public class FieldAngularCoordinates<T extends RealFieldElement<T>> {
* have consistent derivation orders.
* </p>
* @param r rotation with time-derivatives embedded within the coordinates
* @since 9.2
*/
public FieldAngularCoordinates(final FieldRotation<FieldDerivativeStructure<T>> r) {
......
......@@ -26,23 +26,33 @@ import java.util.ArrayList;
import java.util.List;
import java.util.function.BiFunction;
import org.hipparchus.Field;
import org.hipparchus.RealFieldElement;
import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
import org.hipparchus.geometry.euclidean.threed.Vector3D;
import org.hipparchus.util.Decimal64;
import org.hipparchus.util.Decimal64Field;
import org.hipparchus.util.FastMath;
import org.junit.Assert;
import org.junit.Before;
import org.orekit.Utils;
import org.orekit.attitudes.Attitude;
import org.orekit.attitudes.FieldAttitude;