Commit 3b356d56 authored by Luc Maisonobe's avatar Luc Maisonobe

Implemented constellation-specific turns.

We still need to use the computed yaw angle and to add the
derivatives... and of course to validate the models.
parent e2a56b2f
......@@ -44,7 +44,7 @@
<orekit.nexus-staging-maven-plugin.version>1.6.8</orekit.nexus-staging-maven-plugin.version>
<orekit.maven-gpg-plugin.version>1.6</orekit.maven-gpg-plugin.version>
<orekit.maven-install-plugin.version>2.5.2</orekit.maven-install-plugin.version>
<orekit.hipparchus.version>1.2</orekit.hipparchus.version>
<orekit.hipparchus.version>1.3-SNAPSHOT</orekit.hipparchus.version>
<orekit.junit.version>4.12</orekit.junit.version>
<orekit.compiler.source>1.8</orekit.compiler.source>
<orekit.compiler.target>1.8</orekit.compiler.target>
......
......@@ -50,7 +50,7 @@ public class BeidouIGSO extends AbstractGNSSAttitudeProvider {
protected TimeStampedAngularCoordinates correctYaw(final GNSSAttitudeContext context)
throws OrekitException {
if (FastMath.abs(context.getBeta().getValue()) < 2 * BETA_0) {
if (FastMath.abs(context.getBeta()) < 2 * BETA_0) {
// when Sun is close to orbital plane, attitude is in Orbit Normal (ON) yaw
return context.orbitNormalYaw();
}
......
......@@ -50,7 +50,7 @@ public class BeidouMeo extends AbstractGNSSAttitudeProvider {
protected TimeStampedAngularCoordinates correctYaw(final GNSSAttitudeContext context)
throws OrekitException {
if (FastMath.abs(context.getBeta().getValue()) < 2 * BETA_0) {
if (FastMath.abs(context.getBeta()) < 2 * BETA_0) {
// when Sun is close to orbital plane, attitude is in Orbit Normal (ON) yaw
return context.orbitNormalYaw();
}
......
......@@ -53,6 +53,9 @@ public class GPSBlockIIA extends AbstractGNSSAttitudeProvider {
0.1001, 0.1227, 0.1194, 0.1260, 0.1228, 0.1165, 0.0969, 0.1140
};
/** Margin on turn end. */
private final double END_MARGIN = 1800.0;
/** Yaw rate for current spacecraft. */
private final double yawRate;
......@@ -78,15 +81,57 @@ public class GPSBlockIIA extends AbstractGNSSAttitudeProvider {
final double cNoon = FastMath.cos(aNoon);
final double cNight = FastMath.cos(aNight);
if (context.inTurnRegion(cNight, cNoon)) {
if (context.setUpTurnRegion(cNight, cNoon)) {
final double absBeta = FastMath.abs(context.getBeta());
context.setHalfSpan(context.inSunSide() ?
absBeta * FastMath.sqrt(aNoon / absBeta - 1.0) :
context.inOrbitPlaneAngle(aNight - FastMath.PI));
if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
// we need to ensure beta sign does not change during the turn
final double beta = context.getSecuredBeta();
final double phiStart = context.getYawStart(beta);
final double dtStart = context.timeSinceTurnStart(context.getDate());
final double phi;
if (context.inSunSide()) {
// noon turn
final double linearPhi;
if (beta > 0 && beta < YAW_BIAS) {
// noon turn problem for small positive beta in block IIA
// rotation is in the wrong direction for these spacecrafts
linearPhi = phiStart + FastMath.copySign(yawRate, beta) * dtStart;
} else {
// regular noon turn
linearPhi = phiStart - FastMath.copySign(yawRate, beta) * dtStart;
}
// TODO: there is no protection against overshooting phiEnd as in night turn
// there should probably be some protection
phi = linearPhi;
} else {
// midnight turn
final double dtEnd = dtStart - context.getTurnDuration();
if (dtEnd < 0) {
// we are within the turn itself
phi = phiStart + yawRate * dtStart;
} else {
// we are in the recovery phase after turn
final double phiEnd = phiStart + yawRate * context.getTurnDuration();
final double deltaPhi = context.yawAngle() - phiEnd;
if (FastMath.abs(deltaPhi / yawRate) <= dtEnd) {
// 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
phi = phiEnd + FastMath.copySign(yawRate * dtEnd, deltaPhi);
}
}
}
final double absBeta = FastMath.abs(context.getBeta().getReal());
final TurnTimeRange turnTimeRange = context.turnTimeRange(context.inSunSide() ?
absBeta * FastMath.sqrt(aNoon / absBeta - 1.0) :
context.inOrbitPlaneAngle(aNight - FastMath.PI));
if (turnTimeRange.inRange(context.getDate())) {
// TODO
return null;
}
}
......
......@@ -48,6 +48,9 @@ public class GPSBlockIIF extends AbstractGNSSAttitudeProvider {
/** Yaw rates for all spacecrafts. */
private static final double YAW_RATE = FastMath.toRadians(0.11);
/** Margin on turn end. */
private final double END_MARGIN = 1800.0;
/** Simple constructor.
* @param validityStart start of validity for this provider
* @param validityEnd end of validity for this provider
......@@ -68,15 +71,42 @@ public class GPSBlockIIF extends AbstractGNSSAttitudeProvider {
final double cNoon = FastMath.cos(aNoon);
final double cNight = FastMath.cos(aNight);
if (context.inTurnRegion(cNight, cNoon)) {
if (context.setUpTurnRegion(cNight, cNoon)) {
final double absBeta = FastMath.abs(context.getBeta());
context.setHalfSpan(context.inSunSide() ?
absBeta * FastMath.sqrt(aNoon / absBeta - 1.0) :
context.inOrbitPlaneAngle(aNight - FastMath.PI));
if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
// we need to ensure beta sign does not change during the turn
final double beta = context.getSecuredBeta();
final double phiStart = context.getYawStart(beta);
final double dtStart = context.timeSinceTurnStart(context.getDate());
final double phi;
if (context.inSunSide()) {
// noon turn
final double linearPhi;
if (beta > YAW_BIAS && beta < 0) {
// noon turn problem for small negative beta in block IIF
// rotation is in the wrong direction for these spacecrafts
linearPhi = phiStart + FastMath.copySign(YAW_RATE, beta) * dtStart;
} else {
// regular noon turn
linearPhi = phiStart - FastMath.copySign(YAW_RATE, beta) * dtStart;
}
// TODO: there is no protection against overshooting phiEnd as in night turn
// there should probably be some protection
phi = linearPhi;
} else {
// midnight turn
final double yawEnd = context.yawRate(beta);
phi = phiStart - yawEnd * dtStart;
}
final double absBeta = FastMath.abs(context.getBeta().getReal());
final TurnTimeRange turnTimeRange = context.turnTimeRange(context.inSunSide() ?
absBeta * FastMath.sqrt(aNoon / absBeta - 1.0) :
context.inOrbitPlaneAngle(aNight - FastMath.PI));
if (turnTimeRange.inRange(context.getDate())) {
// TODO
return null;
}
}
......
......@@ -45,6 +45,9 @@ public class GPSBlockIIR extends AbstractGNSSAttitudeProvider {
/** Yaw rates for all spacecrafts. */
private static final double YAW_RATE = FastMath.toRadians(0.2);
/** Margin on turn end. */
private final double END_MARGIN = 1800.0;
/** Simple constructor.
* @param validityStart start of validity for this provider
* @param validityEnd end of validity for this provider
......@@ -64,13 +67,35 @@ public class GPSBlockIIR extends AbstractGNSSAttitudeProvider {
final double cNoon = FastMath.cos(aNoon);
final double cNight = -cNoon;
if (context.inTurnRegion(cNight, cNoon)) {
if (context.setUpTurnRegion(cNight, cNoon)) {
final double absBeta = FastMath.abs(context.getBeta());
context.setHalfSpan(absBeta * FastMath.sqrt(aNoon / absBeta - 1.0));
if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
// we need to ensure beta sign does not change during the turn
final double beta = context.getSecuredBeta();
final double phiStart = context.getYawStart(beta);
final double dtStart = context.timeSinceTurnStart(context.getDate());
final double phi;
if (context.inSunSide()) {
// noon turn
final double linearPhi = phiStart - FastMath.copySign(YAW_RATE, beta) * dtStart;
// TODO: there is no protection against overshooting phiEnd as in night turn
// there should probably be some protection
phi = linearPhi;
} else {
// midnight turn
final double linearPhi = phiStart + FastMath.copySign(YAW_RATE, beta) * dtStart;
final double phiEnd = context.getYawEnd(beta);
// TODO: the part "phiEnd / linearPhi < 0" is suspicious and should probably be removed
phi = (phiEnd / linearPhi < 0 || phiEnd / linearPhi > 1) ? phiEnd : linearPhi;
}
final double absBeta = FastMath.abs(context.getBeta().getReal());
final TurnTimeRange turnTimeRange = context.turnTimeRange(absBeta * FastMath.sqrt(aNoon / absBeta - 1.0));
if (turnTimeRange.inRange(context.getDate())) {
// TODO
return null;
}
}
......
......@@ -16,8 +16,8 @@
*/
package org.orekit.gnss.attitude;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.SinCos;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
import org.orekit.utils.TimeStampedAngularCoordinates;
......@@ -52,8 +52,8 @@ public class Galileo extends AbstractGNSSAttitudeProvider {
/** Limit for the night turn. */
private static final double COS_NIGHT = -COS_NOON;
/** Yaw rates for all spacecrafts. */
private static final double YAW_RATE = FastMath.toRadians(0.203);
/** No margin on turn end for Galileo. */
private final double END_MARGIN = 0.0;
/** Simple constructor.
* @param validityStart start of validity for this provider
......@@ -69,14 +69,25 @@ public class Galileo extends AbstractGNSSAttitudeProvider {
@Override
protected TimeStampedAngularCoordinates correctYaw(final GNSSAttitudeContext context) {
final DerivativeStructure beta = context.getBeta();
if (FastMath.abs(context.getBeta()) < BETA_Y &&
context.setUpTurnRegion(COS_NIGHT, COS_NOON)) {
context.setHalfSpan(context.inSunSide() ?
BETA_X :
context.inOrbitPlaneAngle(BETA_X));
if (context.inTurnTimeRange(context.getDate(), END_MARGIN)) {
// handling both noon and midnight turns at once
final double beta = context.getBeta();
final SinCos scBeta = FastMath.sinCos(beta);
final double sinY = FastMath.copySign(FastMath.sin(BETA_Y), context.getSecuredBeta());
final double sd = FastMath.copySign(FastMath.sin(context.getDelta()), context.getSVBcos());
final double c = sd * scBeta.cos();
final double shy = 0.5 * ((-sinY - scBeta.sin()) +
(-sinY + scBeta.sin()) *
FastMath.cos(FastMath.PI * FastMath.abs(c) / FastMath.sin(BETA_X)));
final double phi = FastMath.atan2(shy, c);
if (FastMath.abs(beta.getValue()) < BETA_Y && context.inTurnRegion(COS_NIGHT, COS_NOON)) {
final TurnTimeRange turnTimeRange = context.turnTimeRange(context.inSunSide() ?
BETA_X :
context.inOrbitPlaneAngle(BETA_X));
if (turnTimeRange.inRange(context.getDate())) {
// TODO
return null;
}
......
......@@ -16,7 +16,6 @@
*/
package org.orekit.gnss.attitude;
import org.hipparchus.analysis.differentiation.DerivativeStructure;
import org.hipparchus.util.FastMath;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.PVCoordinatesProvider;
......@@ -64,16 +63,16 @@ public class Glonass extends AbstractGNSSAttitudeProvider {
protected TimeStampedAngularCoordinates correctYaw(final GNSSAttitudeContext context) {
// noon beta angle limit from yaw rate
final DerivativeStructure beta = context.getBeta();
final double muRate = context.getMuRate();
final double aNight = NIGHT_TURN_LIMIT;
double aNoon = FastMath.atan(muRate / YAW_RATE);
if (FastMath.abs(beta.getValue()) < aNoon) {
final double realBeta = context.getBeta();
final double muRate = context.getMuRate();
final double aNight = NIGHT_TURN_LIMIT;
double aNoon = FastMath.atan(muRate / YAW_RATE);
if (FastMath.abs(realBeta) < aNoon) {
double yawEnd = YAW_END_ZERO;
final double tan = FastMath.tan(beta.getValue());
for (int i = 0; i < 3; ++i) {
final double sin = FastMath.sin(muRate * yawEnd / YAW_RATE);
yawEnd = 0.5 * FastMath.abs(FastMath.atan2(-tan, sin) - FastMath.atan2(-tan, -sin));
final double delta = muRate * yawEnd / YAW_RATE;
yawEnd = 0.5 * FastMath.abs(context.computePhi(realBeta, delta) -
context.computePhi(realBeta, -delta));
}
aNoon = muRate * yawEnd / YAW_RATE;
}
......@@ -81,14 +80,36 @@ public class Glonass extends AbstractGNSSAttitudeProvider {
final double cNoon = FastMath.cos(aNoon);
final double cNight = FastMath.cos(aNight);
if (context.inTurnRegion(cNight, cNoon)) {
if (context.setUpTurnRegion(cNight, cNoon)) {
context.setHalfSpan(context.inSunSide() ?
aNoon :
context.inOrbitPlaneAngle(aNight - FastMath.PI));
if (context.inTurnTimeRange(context.getDate(), 0)) {
// we need to ensure beta sign does not change during the turn
final double beta = context.getSecuredBeta();
final double phiStart = context.getYawStart(beta);
final double dtStart = context.timeSinceTurnStart(context.getDate());
final double phi;
if (context.inSunSide()) {
// noon turn
final double linearPhi = phiStart - FastMath.copySign(YAW_RATE, beta) * dtStart;
// TODO: there is no protection against overshooting phiEnd
// there should probably be some protection
phi = linearPhi;
} else {
// midnight turn
final double linearPhi = phiStart + FastMath.copySign(YAW_RATE, beta) * dtStart;
final double phiEnd = context.getYawEnd(beta);
// TODO: the part "phiEnd / linearPhi < 0" is suspicious and should probably be removed
phi = (phiEnd / linearPhi < 0 || phiEnd / linearPhi > 1) ? phiEnd : linearPhi;
}
final TurnTimeRange turnTimeRange = context.turnTimeRange(context.inSunSide() ?
aNoon :
context.inOrbitPlaneAngle(aNight - FastMath.PI));
if (turnTimeRange.inRange(context.getDate())) {
// TODO
return null;
}
}
......
/* Copyright 2002-2017 CS Systèmes d'Information
* Licensed to CS Systèmes d'Information (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.gnss.attitude;
import org.orekit.time.AbsoluteDate;
/** Holder for turn time range.
* @see GNSSAttitudeContext#turnTimeRange()
* @author Luc Maisonobe
* @since 9.2
*/
class TurnTimeRange {
/** Turn start date. */
private final AbsoluteDate start;
/** Turn end date. */
private final AbsoluteDate end;
/** Simple constructor.
* @param start turn start date
* @param end turn end date
*/
TurnTimeRange(final AbsoluteDate start, final AbsoluteDate end) {
this.start = start;
this.end = end;
}
/** Get the turn start date.
* @return turn start date
*/
public AbsoluteDate getStart() {
return start;
}
/** Get the turn end date.
* @return turn end date
*/
public AbsoluteDate getEnd() {
return end;
}
/** Check if a date is within range.
* @param date date to check
* @return true if date is within range
*/
public boolean inRange(final AbsoluteDate date) {
return date.compareTo(start) > 0 && date.compareTo(end) < 0;
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment