GNSSAttitudeContext.java

  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;

  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;

  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 {

  52.     /** Constant Y axis. */
  53.     private static final PVCoordinates PLUS_Y =
  54.             new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO);

  56.     /** Constant Z axis. */
  57.     private static final PVCoordinates MINUS_Z =
  58.             new PVCoordinates(Vector3D.MINUS_K, Vector3D.ZERO, Vector3D.ZERO);

  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);

  63.     /** Derivation order. */
  64.     private static final int ORDER = 2;

  66.     /** Spacecraft position-velocity in inertial frame. */
  67.     private final TimeStampedPVCoordinates svPV;

  69.     /** Spacecraft position-velocity in inertial frame. */
  70.     private final FieldPVCoordinates<DerivativeStructure> svPVDS;

  72.     /** Angle between Sun and orbital plane. */
  73.     private final DerivativeStructure beta;

  75.     /** Cosine of the angle between spacecraft and Sun direction. */
  76.     private final DerivativeStructure svbCos;

  78.     /** Nominal yaw. */
  79.     private final TimeStampedAngularCoordinates nominalYaw;

  81.     /** Nominal yaw. */
  82.     private final FieldRotation<DerivativeStructure> nominalYawDS;

  84.     /** Spacecraft angular velocity. */
  85.     private double muRate;

  87.     /** Limit cosine for the midnight turn. */
  88.     private double cNight;

  90.     /** Limit cosine for the noon turn. */
  91.     private double cNoon;

  93.     /** Relative orbit angle to turn center. */
  94.     private DerivativeStructure delta;

  96.     /** Half span of the turn region, as an angle in orbit plane. */
  97.     private double halfSpan;

  99.     /** Turn start date. */
  100.     private AbsoluteDate turnStart;

  102.     /** Turn end date. */
  103.     private AbsoluteDate turnEnd;

  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 {

  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);

  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);

  133.         this.muRate = svPV.getAngularVelocity().getNorm();

  135.     }

  137.     /** {@inheritDoc} */
  138.     @Override
  139.     public AbsoluteDate getDate() {
  140.         return svPV.getDate();
  141.     }

  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.     }

  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.     }

  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.     }

  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.     }

  184.     /** Get the nominal yaw.
  185.      * @return nominal yaw
  186.      */
  187.     public TimeStampedAngularCoordinates getNominalYaw() {
  188.         return nominalYaw;
  189.     }

  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.     }

  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.     }

  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.     }

  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.     }

  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.     }

  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.     }

  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.     }

  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.     }

  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.     }

  281.     /** Get the spacecraft angular velocity.
  282.      * @return spacecraft angular velocity
  283.      */
  284.     public double getMuRate() {
  285.         return muRate;
  286.     }

  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.     }

  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.     }

  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.     }

  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.     }

  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.     }

  346.     /** Get turn duration.
  347.      * @return turn duration
  348.      */
  349.     public double getTurnDuration() {
  350.         return 2 * halfSpan / muRate;
  351.     }

  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.     }

  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.     }

  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) {

  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));

  384.         // combine the two parts of the attitude
  385.         return correction.addOffset(getNominalYaw());

  387.     }

  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.     }

  400. }
Created with JaCoCo 0.7.9.201702052155