AbstractGNSSAttitudeProvider.java

  1. /* Copyright 2002-2023 CS GROUP
  2.  * Licensed to CS GROUP (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 java.util.HashMap;
  20. import java.util.Map;
  21. import java.util.SortedSet;
  22. import java.util.TreeSet;

  24. import org.hipparchus.Field;
  25. import org.hipparchus.CalculusFieldElement;
  26. import org.orekit.attitudes.Attitude;
  27. import org.orekit.attitudes.FieldAttitude;
  28. import org.orekit.frames.Frame;
  29. import org.orekit.time.AbsoluteDate;
  30. import org.orekit.time.ChronologicalComparator;
  31. import org.orekit.time.FieldAbsoluteDate;
  32. import org.orekit.time.TimeStamped;
  33. import org.orekit.utils.ExtendedPVCoordinatesProvider;
  34. import org.orekit.utils.FieldPVCoordinatesProvider;
  35. import org.orekit.utils.PVCoordinatesProvider;
  36. import org.orekit.utils.TimeStampedAngularCoordinates;
  37. import org.orekit.utils.TimeStampedFieldAngularCoordinates;

  39. /**
  40.  * Base class for attitude providers for navigation satellites.
  41.  *
  42.  * @author Luc Maisonobe
  43.  * @since 9.2
  44.  */
  45. abstract class AbstractGNSSAttitudeProvider implements GNSSAttitudeProvider {

  47.     /** Start of validity for this provider. */
  48.     private final AbsoluteDate validityStart;

  50.     /** End of validity for this provider. */
  51.     private final AbsoluteDate validityEnd;

  53.     /** Provider for Sun position. */
  54.     private final ExtendedPVCoordinatesProvider sun;

  56.     /** Inertial frame where velocity are computed. */
  57.     private final Frame inertialFrame;

  59.     /** Turns already encountered. */
  60.     private final SortedSet<TimeStamped> turns;

  62.     /** Turns already encountered. */
  63.     private final transient Map<Field<? extends CalculusFieldElement<?>>, SortedSet<TimeStamped>> fieldTurns;

  65.     /** Simple constructor.
  66.      * @param validityStart start of validity for this provider
  67.      * @param validityEnd end of validity for this provider
  68.      * @param sun provider for Sun position
  69.      * @param inertialFrame inertial frame where velocity are computed
  70.      */
  71.     protected AbstractGNSSAttitudeProvider(final AbsoluteDate validityStart,
  72.                                            final AbsoluteDate validityEnd,
  73.                                            final ExtendedPVCoordinatesProvider sun,
  74.                                            final Frame inertialFrame) {
  75.         this.validityStart = validityStart;
  76.         this.validityEnd   = validityEnd;
  77.         this.sun           = sun;
  78.         this.inertialFrame = inertialFrame;
  79.         this.turns         = new TreeSet<>(new ChronologicalComparator());
  80.         this.fieldTurns    = new HashMap<>();
  81.     }

  83.     /** {@inheritDoc} */
  84.     @Override
  85.     public AbsoluteDate validityStart() {
  86.         return validityStart;
  87.     }

  89.     /** {@inheritDoc} */
  90.     @Override
  91.     public AbsoluteDate validityEnd() {
  92.         return validityEnd;
  93.     }

  95.     /** {@inheritDoc} */
  96.     @Override
  97.     public Attitude getAttitude(final PVCoordinatesProvider pvProv,
  98.                                 final AbsoluteDate date,
  99.                                 final Frame frame) {

  101.         // compute yaw correction
  102.         final TurnSpan                      turnSpan  = getTurnSpan(date);
  103.         final GNSSAttitudeContext           context   = new GNSSAttitudeContext(date, sun, pvProv, inertialFrame, turnSpan);
  104.         final TimeStampedAngularCoordinates corrected = correctedYaw(context);
  105.         if (turnSpan == null && context.getTurnSpan() != null) {
  106.             // we have encountered a new turn, store it
  107.             turns.add(context.getTurnSpan());
  108.         }

  110.         return new Attitude(inertialFrame, corrected).withReferenceFrame(frame);

  112.     }

  114.     /** {@inheritDoc} */
  115.     @Override
  116.     public <T extends CalculusFieldElement<T>> FieldAttitude<T> getAttitude(final FieldPVCoordinatesProvider<T> pvProv,
  117.                                                                             final FieldAbsoluteDate<T> date,
  118.                                                                             final Frame frame) {

  120.         // compute yaw correction
  121.         final FieldTurnSpan<T>                      turnSpan  = getTurnSpan(date);
  122.         final GNSSFieldAttitudeContext<T>           context   = new GNSSFieldAttitudeContext<>(date, sun, pvProv, inertialFrame, turnSpan);
  123.         final TimeStampedFieldAngularCoordinates<T> corrected = correctedYaw(context);
  124.         if (turnSpan == null && context.getTurnSpan() != null) {
  125.             // we have encountered a new turn, store it
  126.             fieldTurns.get(date.getField()).add(context.getTurnSpan());
  127.         }

  129.         return new FieldAttitude<>(inertialFrame, corrected).withReferenceFrame(frame);

  131.     }

  133.     /** Get the turn span covering a date.
  134.      * @param date date to check
  135.      * @return turn span covering the date, or null if no span covers this date
  136.      */
  137.     private TurnSpan getTurnSpan(final AbsoluteDate date) {

  139.         // as the reference date of the turn span is the end + margin date,
  140.         // the span to consider can only be the first span that is after date
  141.         final SortedSet<TimeStamped> after = turns.tailSet(date);
  142.         if (!after.isEmpty()) {
  143.             final TurnSpan ts = (TurnSpan) after.first();
  144.             if (ts.inTurnTimeRange(date)) {
  145.                 return ts;
  146.             }
  147.         }

  149.         // no turn covers the date
  150.         return null;

  152.     }

  154.     /** Get the turn span covering a date.
  155.      * @param date date to check
  156.      * @param <T> type of the field elements
  157.      * @return turn span covering the date, or null if no span covers this date
  158.      */
  159.     private <T extends CalculusFieldElement<T>> FieldTurnSpan<T> getTurnSpan(final FieldAbsoluteDate<T> date) {

  161.         SortedSet<TimeStamped> sortedSet = fieldTurns.get(date.getField());
  162.         if (sortedSet == null) {
  163.             // this is the first time we manage such a field, prepare a sorted set for it
  164.             sortedSet = new TreeSet<>(new ChronologicalComparator());
  165.             fieldTurns.put(date.getField(), sortedSet);
  166.         }

  168.         // as the reference date of the turn span is the end + margin date,
  169.         // the span to consider can only be the first span that is after date
  170.         final AbsoluteDate dateDouble = date.toAbsoluteDate();
  171.         final SortedSet<TimeStamped> after = sortedSet.tailSet(dateDouble);
  172.         if (!after.isEmpty()) {
  173.             @SuppressWarnings("unchecked")
  174.             final FieldTurnSpan<T> ts = (FieldTurnSpan<T>) after.first();
  175.             if (ts.inTurnTimeRange(dateDouble)) {
  176.                 return ts;
  177.             }
  178.         }

  180.         // no turn covers the date
  181.         return null;

  183.     }

  185.     /** Get provider for Sun position.
  186.      * @return provider for Sun position
  187.      * @since 12.0
  188.      */
  189.     protected ExtendedPVCoordinatesProvider getSun() {
  190.         return sun;
  191.     }

  193.     /** Get inertial frame where velocity are computed.
  194.      * @return inertial frame where velocity are computed
  195.      */
  196.     protected Frame getInertialFrame() {
  197.         return inertialFrame;
  198.     }

  200.     /** Select the
  201.     /** Compute GNSS attitude with midnight/noon yaw turn correction.
  202.      * @param context context data for attitude computation
  203.      * @return corrected yaw, using inertial frame as the reference
  204.      */
  205.     protected abstract TimeStampedAngularCoordinates correctedYaw(GNSSAttitudeContext context);

  207.     /** Compute GNSS attitude with midnight/noon yaw turn correction.
  208.      * @param context context data for attitude computation
  209.      * @param <T> type of the field elements
  210.      * @return corrected yaw, using inertial frame as the reference
  211.      */
  212.     protected abstract <T extends CalculusFieldElement<T>> TimeStampedFieldAngularCoordinates<T>
  213.         correctedYaw(GNSSFieldAttitudeContext<T> context);

  215. }
Created with JaCoCo 0.8.11.202310140853