1   /* Copyright 2022-2025 Luc Maisonobe
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  
18  package org.orekit.files.ccsds.ndm.adm.acm;
19  
20  import java.util.ArrayList;
21  import java.util.Collections;
22  import java.util.List;
23  
24  import org.hipparchus.geometry.euclidean.threed.RotationOrder;
25  import org.orekit.errors.OrekitException;
26  import org.orekit.errors.OrekitMessages;
27  import org.orekit.files.ccsds.definitions.AdMethodType;
28  import org.orekit.files.ccsds.ndm.adm.AttitudeEndpoints;
29  import org.orekit.files.ccsds.section.CommentsContainer;
30  
31  /** Attitude determination data.
32   * <p>
33   * Beware that the Orekit getters and setters all rely on SI units. The parsers
34   * and writers take care of converting these SI units into CCSDS mandatory units.
35   * The {@link org.orekit.utils.units.Unit Unit} class provides useful
36   * {@link org.orekit.utils.units.Unit#fromSI(double) fromSi} and
37   * {@link org.orekit.utils.units.Unit#toSI(double) toSI} methods in case the callers
38   * already use CCSDS units instead of the API SI units. The general-purpose
39   * {@link org.orekit.utils.units.Unit Unit} class (without an 's') and the
40   * CCSDS-specific {@link org.orekit.files.ccsds.definitions.Units Units} class
41   * (with an 's') also provide some predefined units. These predefined units and the
42   * {@link org.orekit.utils.units.Unit#fromSI(double) fromSi} and
43   * {@link org.orekit.utils.units.Unit#toSI(double) toSI} conversion methods are indeed
44   * what the parsers and writers use for the conversions.
45   * </p>
46   * @author Luc Maisonobe
47   * @since 12.0
48   */
49  public class AttitudeDetermination extends CommentsContainer {
50  
51      /** Endpoints (i.e. frames A, B and their relationship). */
52      private final AttitudeEndpoints endpoints;
53  
54      /** Identification number. */
55      private String id;
56  
57      /** Identification of previous orbit determination. */
58      private String prevId;
59  
60      /** Attitude determination method. */
61      private AdMethodType method;
62  
63      /** Source of attitude estimate. */
64      private String source;
65  
66      /** Rotation order for Euler angles. */
67      private RotationOrder eulerRotSeq;
68  
69      /** Number of states for {@link AdMethodType#EKF}, {@link AdMethodType#BATCH} or {@link AdMethodType#FILTER_SMOOTHER}. */
70      private int nbStates;
71  
72      /** Attitude states. */
73      private AttitudeElementsType attitudeStates;
74  
75      /** Type of attitude error state. */
76      private AttitudeCovarianceType covarianceType;
77  
78      /** Attitude rate states. */
79      private RateElementsType rateStates;
80  
81      /** Rate random walk if {@link #rateStates} is {@link RateElementsType#GYRO_BIAS}. */
82      private double sigmaU;
83  
84      /** Angle random walk if {@link #rateStates} is {@link RateElementsType#GYRO_BIAS}. */
85      private double sigmaV;
86  
87      /** Process noise standard deviation if {@link #rateStates} is {@link RateElementsType#ANGVEL}. */
88      private double rateProcessNoiseStdDev;
89  
90      /** Sensors used. */
91      private final List<AttitudeDeterminationSensor> sensorsUsed;
92  
93      /** Simple constructor.
94       */
95      public AttitudeDetermination() {
96          endpoints   = new AttitudeEndpoints();
97          sensorsUsed = new ArrayList<>();
98          nbStates    = -1;
99      }
100 
101     /** {@inheritDoc} */
102     @Override
103     public void validate(final double version) {
104         super.validate(version);
105         checkNotNull(attitudeStates, AttitudeDeterminationKey.ATTITUDE_STATES.name());
106         endpoints.checkExternalFrame(AttitudeDeterminationKey.REF_FRAME_A,
107                                      AttitudeDeterminationKey.REF_FRAME_B);
108 
109         // check sensors in increasing number
110         for (int number = 1; number <= sensorsUsed.size(); ++number) {
111             final AttitudeDeterminationSensor sensor = findSensor(number);
112             if (sensor != null) {
113                 sensor.validate(version);
114             } else {
115                 // no sensor has the expected index
116                 throw new OrekitException(OrekitMessages.CCSDS_MISSING_SENSOR_INDEX, number);
117             }
118 
119         }
120 
121     }
122 
123     /** Find sensor by number.
124      * @param number number of the sensor
125      * @return sensor with specified number, or null if not found
126      */
127     private AttitudeDeterminationSensor findSensor(final int number) {
128         for (final AttitudeDeterminationSensor sensor : sensorsUsed) {
129             if (sensor.getSensorNumber() == number) {
130                 return sensor;
131             }
132         }
133         return null;
134     }
135 
136     /** Get the endpoints (i.e. frames A, B and their relationship).
137      * @return endpoints
138      */
139     public AttitudeEndpoints getEndpoints() {
140         return endpoints;
141     }
142 
143     /** Get identification number.
144      * @return identification number
145      */
146     public String getId() {
147         return id;
148     }
149 
150     /** Set identification number.
151      * @param id identification number
152      */
153     public void setId(final String id) {
154         this.id = id;
155     }
156 
157     /** Get identification of previous orbit determination.
158      * @return identification of previous orbit determination
159      */
160     public String getPrevId() {
161         return prevId;
162     }
163 
164     /** Set identification of previous orbit determination.
165      * @param prevId identification of previous orbit determination
166      */
167     public void setPrevId(final String prevId) {
168         this.prevId = prevId;
169     }
170 
171     /** Get attitude determination method.
172      * @return attitude determination method
173      */
174     public AdMethodType getMethod() {
175         return method;
176     }
177 
178     /** Set attitude determination method.
179      * @param method attitude determination method
180      */
181     public void setMethod(final AdMethodType method) {
182         this.method = method;
183     }
184 
185     /** Get source of attitude estimate.
186      * @return source of attitude estimate
187      */
188     public String getSource() {
189         return source;
190     }
191 
192     /** Set source of attitude estimate.
193      * @param source source of attitude estimate
194      */
195     public void setSource(final String source) {
196         this.source = source;
197     }
198 
199     /** Get the rotation order for Euler angles.
200      * @return rotation order for Euler angles
201      */
202     public RotationOrder getEulerRotSeq() {
203         return eulerRotSeq;
204     }
205 
206     /** Set the rotation order for Euler angles.
207      * @param eulerRotSeq rotation order for Euler angles
208      */
209     public void setEulerRotSeq(final RotationOrder eulerRotSeq) {
210         this.eulerRotSeq = eulerRotSeq;
211     }
212 
213     /** Get number of states for {@link AdMethodType#EKF}, {@link AdMethodType#BATCH} or {@link AdMethodType#FILTER_SMOOTHER}.
214      * @return number of states
215      */
216     public int getNbStates() {
217         return nbStates;
218     }
219 
220     /** Set number of states for {@link AdMethodType#EKF}, {@link AdMethodType#BATCH} or {@link AdMethodType#FILTER_SMOOTHER}.
221      * @param nbStates number of states
222      */
223     public void setNbStates(final int nbStates) {
224         this.nbStates = nbStates;
225     }
226 
227     /** Get attitude states.
228      * @return attitude states
229      */
230     public AttitudeElementsType getAttitudeStates() {
231         return attitudeStates;
232     }
233 
234     /** Set attitude states.
235      * @param attitudeStates attitude states
236      */
237     public void setAttitudeStates(final AttitudeElementsType attitudeStates) {
238         this.attitudeStates = attitudeStates;
239     }
240 
241     /** Get type of attitude error state.
242      * @return type of attitude error state
243      */
244     public AttitudeCovarianceType getCovarianceType() {
245         return covarianceType;
246     }
247 
248     /** Set type of attitude error state.
249      * @param covarianceType type of attitude error state
250      */
251     public void setCovarianceType(final AttitudeCovarianceType covarianceType) {
252         this.covarianceType = covarianceType;
253     }
254 
255     /** Get attitude rate states.
256      * @return attitude rate states
257      */
258     public RateElementsType getRateStates() {
259         return rateStates;
260     }
261 
262     /** Set attitude rate states.
263      * @param rateStates attitude rate states
264      */
265     public void setRateStates(final RateElementsType rateStates) {
266         this.rateStates = rateStates;
267     }
268 
269     /** Get rate random walk if {@link #rateStates} is {@link RateElementsType#GYRO_BIAS}.
270      * @return rate random walk if {@link #rateStates} is {@link RateElementsType#GYRO_BIAS}
271      */
272     public double getSigmaU() {
273         return sigmaU;
274     }
275 
276     /** Set rate random walk if {@link #rateStates} is {@link RateElementsType#GYRO_BIAS}.
277      * @param sigmaU rate random walk if {@link #rateStates} is {@link RateElementsType#GYRO_BIAS}
278      */
279     public void setSigmaU(final double sigmaU) {
280         this.sigmaU = sigmaU;
281     }
282 
283     /** Get angle random walk if {@link #rateStates} is {@link RateElementsType#GYRO_BIAS}.
284      * @return angle random walk if {@link #rateStates} is {@link RateElementsType#GYRO_BIAS}
285      */
286     public double getSigmaV() {
287         return sigmaV;
288     }
289 
290     /** Set angle random walk if {@link #rateStates} is {@link RateElementsType#GYRO_BIAS}.
291      * @param sigmaV angle random walk if {@link #rateStates} is {@link RateElementsType#GYRO_BIAS}
292      */
293     public void setSigmaV(final double sigmaV) {
294         this.sigmaV = sigmaV;
295     }
296 
297     /** Get process noise standard deviation if {@link #rateStates} is {@link RateElementsType#ANGVEL}.
298      * @return process noise standard deviation if {@link #rateStates} is {@link RateElementsType#ANGVEL}
299      */
300     public double getRateProcessNoiseStdDev() {
301         return rateProcessNoiseStdDev;
302     }
303 
304     /** Set process noise standard deviation if {@link #rateStates} is {@link RateElementsType#ANGVEL}.
305      * @param rateProcessNoiseStdDev process noise standard deviation if {@link #rateStates} is {@link RateElementsType#ANGVEL}
306      */
307     public void setRateProcessNoiseStdDev(final double rateProcessNoiseStdDev) {
308         this.rateProcessNoiseStdDev = rateProcessNoiseStdDev;
309     }
310 
311     /** Get sensors used.
312      * @return sensors used
313      */
314     public List<AttitudeDeterminationSensor> getSensorsUsed() {
315         return Collections.unmodifiableList(sensorsUsed);
316     }
317 
318     /** Add a sensor used.
319      * @param sensor sensor to add
320      */
321     public void addSensor(final AttitudeDeterminationSensor sensor) {
322         for (final AttitudeDeterminationSensor existing : sensorsUsed) {
323             if (sensor.getSensorNumber() == existing.getSensorNumber()) {
324                 throw new OrekitException(OrekitMessages.CCSDS_SENSOR_INDEX_ALREADY_USED, sensor.getSensorNumber());
325             }
326         }
327         sensorsUsed.add(sensor);
328     }
329 
330 }