Commit 4b2926af authored by Luc Maisonobe's avatar Luc Maisonobe

Merge branch 'gnss-attitude' into develop

parents 659137ab 4296a0b0
......@@ -18,18 +18,15 @@ package org.orekit.bodies;
import java.io.Serializable;
import org.hipparchus.RealFieldElement;
import org.orekit.errors.OrekitException;
import org.orekit.frames.Frame;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.TimeStampedFieldPVCoordinates;
import org.orekit.utils.ExtendedPVCoordinatesProvider;
/** Interface for celestial bodies like Sun, Moon or solar system planets.
* @author Luc Maisonobe
* @see CelestialBodyFactory
*/
public interface CelestialBody extends Serializable, PVCoordinatesProvider {
public interface CelestialBody extends Serializable, ExtendedPVCoordinatesProvider {
/** Get an inertially oriented, body centered frame.
* <p>The frame is always bound to the body center, and its axes have a
......@@ -59,19 +56,4 @@ public interface CelestialBody extends Serializable, PVCoordinatesProvider {
*/
double getGM();
/**
* Get the {@link TimeStampedFieldPVCoordinates} of the body in the selected frame.
*
* @param date current date
* @param frame the frame where to define the position
* @param <T> type fo the field elements
* @return time-stamped position/velocity of the body (m and m/s)
* @throws OrekitException if position cannot be computed in given frame
* @see #getPVCoordinates(org.orekit.time.AbsoluteDate, Frame)
*/
<T extends RealFieldElement<T>> TimeStampedFieldPVCoordinates<T> getPVCoordinates(
FieldAbsoluteDate<T> date,
Frame frame)
throws OrekitException;
}
......@@ -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();
}
}
......@@ -130,9 +130,7 @@ class GNSSAttitudeContext implements TimeStamped {
1.0e-9);
this.nominalYawDS = nominalYaw.toDerivativeStructureRotation(ORDER);
// TODO: the Kouba model assumes perfectly circular orbit, it should really be:
this.muRate = svPV.getAngularVelocity().getNorm();
//this.muRate = svPV.getVelocity().getNorm() / svPV.getPosition().getNorm();
}
......@@ -297,9 +295,7 @@ class GNSSAttitudeContext implements TimeStamped {
* @return angle projected into orbital plane, always positive
*/
private DerivativeStructure inOrbitPlaneAbsoluteAngle(final DerivativeStructure angle) {
// TODO: the Kouba model assumes planar right-angle triangle resolution, it should really be:
return FastMath.acos(FastMath.cos(angle).divide(FastMath.cos(beta)));
//return angle.multiply(angle).subtract(beta.multiply(beta)).sqrt();
}
/** Project a spacecraft/Sun angle into orbital plane.
......@@ -313,9 +309,7 @@ class GNSSAttitudeContext implements TimeStamped {
* @return angle projected into orbital plane, always positive
*/
public double inOrbitPlaneAbsoluteAngle(final double angle) {
// TODO: the Kouba model assumes planar right-angle triangle resolution, it should really be:
return FastMath.acos(FastMath.cos(angle) / FastMath.cos(beta.getReal()));
//return FastMath.sqrt(angle * angle - beta.getReal() * beta.getReal());
}
/** Compute yaw.
......
......@@ -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]);
}
......@@ -109,8 +112,6 @@ public class GPSBlockIIA extends AbstractGNSSAttitudeProvider {
phiDot = -FastMath.copySign(yawRate, beta);
linearPhi = phiStart + phiDot * dtStart;
}
// TODO: there is no protection against overshooting phiEnd as in night turn
// there should probably be some protection
} else {
// midnight turn
final double dtEnd = dtStart - context.getTurnDuration();
......@@ -123,7 +124,7 @@ public class GPSBlockIIA extends AbstractGNSSAttitudeProvider {
phiDot = yawRate;
final double phiEnd = phiStart + phiDot * context.getTurnDuration();
final double deltaPhi = context.yawAngle() - phiEnd;
if (FastMath.abs(deltaPhi / yawRate) <= dtEnd) {
if (FastMath.abs(deltaPhi / phiDot) <= dtEnd) {
// time since turn end was sufficient for recovery
// we are already back in nominal yaw mode
return context.getNominalYaw();
......@@ -145,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);
}
......@@ -99,8 +102,6 @@ public class GPSBlockIIF extends AbstractGNSSAttitudeProvider {
phiDot = -FastMath.copySign(YAW_RATE, beta);
linearPhi = phiStart + phiDot * dtStart;
}
// TODO: there is no protection against overshooting phiEnd as in night turn
// there should probably be some protection
} else {
// midnight turn
phiDot = context.yawRate(beta);
......@@ -118,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.
......@@ -40,9 +43,6 @@ public class GPSBlockIIR extends AbstractGNSSAttitudeProvider {
/** Serializable UID. */
private static final long serialVersionUID = 20171114L;
/** Satellite-Sun angle limit for a midnight turn maneuver. */
private static final double NIGHT_TURN_LIMIT = FastMath.toRadians(180.0 - 13.25);
/** Yaw rates for all spacecrafts. */
private static final double YAW_RATE = FastMath.toRadians(0.2);
......@@ -56,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);
}
......@@ -86,14 +86,11 @@ public class GPSBlockIIR extends AbstractGNSSAttitudeProvider {
// noon turn
phiDot = -FastMath.copySign(YAW_RATE, beta);
linearPhi = phiStart + phiDot * dtStart;
// TODO: there is no protection against overshooting phiEnd as in night turn
// there should probably be some protection
} else {
// midnight turn
phiDot = FastMath.copySign(YAW_RATE, beta);
linearPhi = phiStart + phiDot * dtStart;
final double phiEnd = context.getYawEnd(beta);
// TODO: the part "phiEnd / linearPhi < 0" is suspicious and should probably be removed
if (phiEnd / linearPhi < 0 || phiEnd / linearPhi > 1) {
return context.getNominalYaw();
}
......@@ -110,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();
}
}