nyx_space/cosmic/
spacecraft.rs

1/*
2    Nyx, blazing fast astrodynamics
3    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4
5    This program is free software: you can redistribute it and/or modify
6    it under the terms of the GNU Affero General Public License as published
7    by the Free Software Foundation, either version 3 of the License, or
8    (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13    GNU Affero General Public License for more details.
14
15    You should have received a copy of the GNU Affero General Public License
16    along with this program.  If not, see <https://www.gnu.org/licenses/>.
17*/
18
19use anise::analysis::prelude::OrbitalElement;
20use anise::astro::PhysicsResult;
21use anise::constants::frames::EARTH_J2000;
22pub use anise::prelude::Orbit;
23
24pub use anise::structure::spacecraft::{DragData, Mass, SRPData};
25use der::{Decode, Encode, Enumerated, Reader};
26use nalgebra::Vector3;
27use serde::{Deserialize, Serialize};
28use snafu::ResultExt;
29use typed_builder::TypedBuilder;
30
31use super::{AstroPhysicsSnafu, BPlane, State};
32use crate::cosmic::AstroAnalysisSnafu;
33use crate::dynamics::guidance::{plane_angles_from_unit_vector, LocalFrame, Thruster};
34use crate::dynamics::DynamicsError;
35use crate::errors::{StateAstroSnafu, StateError};
36use crate::io::ConfigRepr;
37use crate::linalg::{Const, DimName, OMatrix, OVector};
38use crate::md::StateParameter;
39use crate::time::Epoch;
40use crate::utils::{cartesian_to_spherical, spherical_to_cartesian};
41
42use std::default::Default;
43use std::fmt;
44use std::ops::Add;
45
46#[cfg(feature = "python")]
47use pyo3::prelude::*;
48
49#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize, Default, Enumerated)]
50#[cfg_attr(feature = "python", pyclass)]
51#[repr(u8)]
52pub enum GuidanceMode {
53    /// Guidance is turned off and Guidance Law may switch mode to Thrust for next call
54    #[default]
55    Coast = 0,
56    /// Guidance is turned on and Guidance Law may switch mode to Coast for next call
57    Thrust = 1,
58    /// Guidance is turned off and Guidance Law may not change its mode (will need to be done externally to the guidance law).
59    Inhibit = 2,
60}
61
62impl From<f64> for GuidanceMode {
63    fn from(value: f64) -> Self {
64        if value >= 1.0 {
65            Self::Thrust
66        } else if value < 0.0 {
67            Self::Inhibit
68        } else {
69            Self::Coast
70        }
71    }
72}
73
74impl From<GuidanceMode> for f64 {
75    fn from(mode: GuidanceMode) -> f64 {
76        match mode {
77            GuidanceMode::Coast => 0.0,
78            GuidanceMode::Thrust => 1.0,
79            GuidanceMode::Inhibit => -1.0,
80        }
81    }
82}
83
84/// Applied thrust direction stored in inertial coordinates for the current propagated state.
85#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize, Default)]
86#[cfg_attr(feature = "python", pyclass)]
87pub struct ThrustDirection {
88    pub x: f64,
89    pub y: f64,
90    pub z: f64,
91}
92
93impl From<Vector3<f64>> for ThrustDirection {
94    fn from(vector: Vector3<f64>) -> Self {
95        // Normalize just in case
96        Self {
97            x: vector[0] / vector.norm(),
98            y: vector[1] / vector.norm(),
99            z: vector[2] / vector.norm(),
100        }
101    }
102}
103
104impl From<ThrustDirection> for Vector3<f64> {
105    fn from(direction: ThrustDirection) -> Self {
106        Vector3::new(direction.x, direction.y, direction.z)
107    }
108}
109
110/// A spacecraft state, composed of its orbit, its masses (dry, prop, extra, all in kg), its SRP configuration, its drag configuration, its thruster configuration, and its guidance mode.
111///
112/// Optionally, the spacecraft state can also store the state transition matrix from the start of the propagation until the current time (i.e. trajectory STM, not step-size STM).
113#[derive(Clone, Copy, Debug, Serialize, Deserialize, TypedBuilder)]
114#[cfg_attr(feature = "python", pyclass)]
115pub struct Spacecraft {
116    /// Initial orbit of the vehicle
117    pub orbit: Orbit,
118    /// Dry, propellant, and extra masses
119    #[builder(default)]
120    pub mass: Mass,
121    /// Solar Radiation Pressure configuration for this spacecraft
122    #[builder(default)]
123    #[serde(default)]
124    pub srp: SRPData,
125    #[builder(default)]
126    #[serde(default)]
127    pub drag: DragData,
128    #[builder(default, setter(strip_option))]
129    pub thruster: Option<Thruster>,
130    /// Any extra information or extension that is needed for specific guidance laws
131    #[builder(default)]
132    #[serde(default)]
133    pub mode: GuidanceMode,
134    /// Optionally stores the applied thrust direction for this state in inertial coordinates.
135    #[builder(default, setter(strip_option))]
136    #[serde(default)]
137    pub thrust_direction: Option<ThrustDirection>,
138    /// Optionally stores the state transition matrix from the start of the propagation until the current time (i.e. trajectory STM, not step-size STM)
139    /// STM is contains position and velocity, Cr, Cd, prop mass
140    #[builder(default, setter(strip_option))]
141    #[serde(skip)]
142    pub stm: Option<OMatrix<f64, Const<9>, Const<9>>>,
143}
144
145impl Default for Spacecraft {
146    fn default() -> Self {
147        Self {
148            orbit: Orbit::zero(EARTH_J2000),
149            mass: Mass::default(),
150            srp: SRPData::default(),
151            drag: DragData::default(),
152            thruster: None,
153            mode: GuidanceMode::default(),
154            thrust_direction: None,
155            stm: None,
156        }
157    }
158}
159
160impl From<Orbit> for Spacecraft {
161    fn from(orbit: Orbit) -> Self {
162        Self::builder().orbit(orbit).build()
163    }
164}
165
166impl Spacecraft {
167    /// Initialize a spacecraft state from all of its parameters
168    pub fn new(
169        orbit: Orbit,
170        dry_mass_kg: f64,
171        prop_mass_kg: f64,
172        srp_area_m2: f64,
173        drag_area_m2: f64,
174        coeff_reflectivity: f64,
175        coeff_drag: f64,
176    ) -> Self {
177        Self {
178            orbit,
179            mass: Mass::from_dry_and_prop_masses(dry_mass_kg, prop_mass_kg),
180            srp: SRPData {
181                area_m2: srp_area_m2,
182                coeff_reflectivity,
183            },
184            drag: DragData {
185                area_m2: drag_area_m2,
186                coeff_drag,
187            },
188            ..Default::default()
189        }
190    }
191
192    /// Initialize a spacecraft state from only a thruster and mass. Use this when designing guidance laws while ignoring drag and SRP.
193    pub fn from_thruster(
194        orbit: Orbit,
195        dry_mass_kg: f64,
196        prop_mass_kg: f64,
197        thruster: Thruster,
198        mode: GuidanceMode,
199    ) -> Self {
200        Self {
201            orbit,
202            mass: Mass::from_dry_and_prop_masses(dry_mass_kg, prop_mass_kg),
203            thruster: Some(thruster),
204            mode,
205            ..Default::default()
206        }
207    }
208
209    /// Initialize a spacecraft state from the SRP default 1.8 for coefficient of reflectivity (prop mass and drag parameters nullified!)
210    pub fn from_srp_defaults(orbit: Orbit, dry_mass_kg: f64, srp_area_m2: f64) -> Self {
211        Self {
212            orbit,
213            mass: Mass::from_dry_mass(dry_mass_kg),
214            srp: SRPData::from_area(srp_area_m2),
215            ..Default::default()
216        }
217    }
218
219    /// Initialize a spacecraft state from the SRP default 1.8 for coefficient of drag (prop mass and SRP parameters nullified!)
220    pub fn from_drag_defaults(orbit: Orbit, dry_mass_kg: f64, drag_area_m2: f64) -> Self {
221        Self {
222            orbit,
223            mass: Mass::from_dry_mass(dry_mass_kg),
224            drag: DragData::from_area(drag_area_m2),
225            ..Default::default()
226        }
227    }
228
229    pub fn with_dv_km_s(mut self, dv_km_s: Vector3<f64>) -> Self {
230        self.orbit.apply_dv_km_s(dv_km_s);
231        self
232    }
233
234    /// Returns a copy of the state with a new dry mass
235    pub fn with_dry_mass(mut self, dry_mass_kg: f64) -> Self {
236        self.mass.dry_mass_kg = dry_mass_kg;
237        self
238    }
239
240    /// Returns a copy of the state with a new prop mass
241    pub fn with_prop_mass(mut self, prop_mass_kg: f64) -> Self {
242        self.mass.prop_mass_kg = prop_mass_kg;
243        self
244    }
245
246    /// Returns a copy of the state with a new SRP area and CR
247    pub fn with_srp(mut self, srp_area_m2: f64, coeff_reflectivity: f64) -> Self {
248        self.srp = SRPData {
249            area_m2: srp_area_m2,
250            coeff_reflectivity,
251        };
252
253        self
254    }
255
256    /// Returns a copy of the state with a new SRP area
257    pub fn with_srp_area(mut self, srp_area_m2: f64) -> Self {
258        self.srp.area_m2 = srp_area_m2;
259        self
260    }
261
262    /// Returns a copy of the state with a new coefficient of reflectivity
263    pub fn with_cr(mut self, coeff_reflectivity: f64) -> Self {
264        self.srp.coeff_reflectivity = coeff_reflectivity;
265        self
266    }
267
268    /// Returns a copy of the state with a new drag area and CD
269    pub fn with_drag(mut self, drag_area_m2: f64, coeff_drag: f64) -> Self {
270        self.drag = DragData {
271            area_m2: drag_area_m2,
272            coeff_drag,
273        };
274        self
275    }
276
277    /// Returns a copy of the state with a new SRP area
278    pub fn with_drag_area(mut self, drag_area_m2: f64) -> Self {
279        self.drag.area_m2 = drag_area_m2;
280        self
281    }
282
283    /// Returns a copy of the state with a new coefficient of drag
284    pub fn with_cd(mut self, coeff_drag: f64) -> Self {
285        self.drag.coeff_drag = coeff_drag;
286        self
287    }
288
289    /// Returns a copy of the state with a new orbit
290    pub fn with_orbit(mut self, orbit: Orbit) -> Self {
291        self.orbit = orbit;
292        self
293    }
294
295    /// Sets the STM of this state of identity, which also enables computation of the STM for spacecraft navigation
296    pub fn enable_stm(&mut self) {
297        self.stm = Some(OMatrix::<f64, Const<9>, Const<9>>::identity());
298    }
299
300    /// Returns the total mass in kilograms
301    pub fn mass_kg(&self) -> f64 {
302        self.mass.total_mass_kg()
303    }
304
305    /// Returns a copy of the state with the provided guidance mode
306    pub fn with_guidance_mode(mut self, mode: GuidanceMode) -> Self {
307        self.mode = mode;
308        self
309    }
310
311    pub fn mode(&self) -> GuidanceMode {
312        self.mode
313    }
314
315    pub fn mut_mode(&mut self, mode: GuidanceMode) {
316        self.mode = mode;
317    }
318
319    /// Return the thrust direction, if there one a predefined one, as a unit vector.
320    pub fn thrust_direction(&self) -> Option<Vector3<f64>> {
321        self.thrust_direction.map(Into::into)
322    }
323
324    /// Set the thrust direction from its unit vector.
325    pub fn mut_thrust_direction(&mut self, direction: Option<Vector3<f64>>) {
326        self.thrust_direction = direction.map(Into::into);
327    }
328
329    /// Return the thrust angles in degrees for the provided frame.
330    /// NOTE: the in-plane and out-of-plane angles differ between the VNC and the RCN frames!
331    pub fn thrust_angles_deg(&self, frame: LocalFrame) -> PhysicsResult<Option<(f64, f64)>> {
332        if let Some(thrust_dir_inertial) = self.thrust_direction() {
333            let dcm_local_to_inertial = frame.dcm_to_inertial(self.orbit)?;
334            let thrust_dir_local = dcm_local_to_inertial.transpose() * thrust_dir_inertial;
335            let (in_plane_rad, out_of_plane_rad) = plane_angles_from_unit_vector(thrust_dir_local);
336
337            Ok(Some((
338                in_plane_rad.to_degrees(),
339                out_of_plane_rad.to_degrees(),
340            )))
341        } else {
342            Ok(None)
343        }
344    }
345}
346
347#[cfg_attr(feature = "python", pymethods)]
348impl Spacecraft {
349    /// Returns the root sum square error between this spacecraft and the other, in kilometers for the position, kilometers per second in velocity, and kilograms in prop
350    pub fn rss(&self, other: &Self) -> PhysicsResult<(f64, f64, f64)> {
351        let rss_p_km = self.orbit.rss_radius_km(&other.orbit)?;
352        let rss_v_km_s = self.orbit.rss_velocity_km_s(&other.orbit)?;
353        let rss_prop_kg = (self.mass.prop_mass_kg - other.mass.prop_mass_kg)
354            .powi(2)
355            .sqrt();
356
357        Ok((rss_p_km, rss_v_km_s, rss_prop_kg))
358    }
359}
360
361impl PartialEq for Spacecraft {
362    fn eq(&self, other: &Self) -> bool {
363        let mass_tol = 1e-6; // milligram
364        self.orbit.eq_within(&other.orbit, 1e-9, 1e-12)
365            && (self.mass - other.mass).abs().total_mass_kg() < mass_tol
366            && self.srp == other.srp
367            && self.drag == other.drag
368    }
369}
370
371#[allow(clippy::format_in_format_args)]
372impl fmt::Display for Spacecraft {
373    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
374        let mass_prec = f.precision().unwrap_or(3);
375        let orbit_prec = f.precision().unwrap_or(6);
376        write!(
377            f,
378            "total mass = {} kg @  {}  {:?}",
379            format!("{:.*}", mass_prec, self.mass.total_mass_kg()),
380            format!("{:.*}", orbit_prec, self.orbit),
381            self.mode,
382        )
383    }
384}
385
386#[allow(clippy::format_in_format_args)]
387impl fmt::LowerExp for Spacecraft {
388    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
389        let mass_prec = f.precision().unwrap_or(3);
390        let orbit_prec = f.precision().unwrap_or(6);
391        write!(
392            f,
393            "total mass = {} kg @  {}  {:?}",
394            format!("{:.*e}", mass_prec, self.mass.total_mass_kg()),
395            format!("{:.*e}", orbit_prec, self.orbit),
396            self.mode,
397        )
398    }
399}
400
401#[allow(clippy::format_in_format_args)]
402impl fmt::LowerHex for Spacecraft {
403    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
404        let mass_prec = f.precision().unwrap_or(3);
405        let orbit_prec = f.precision().unwrap_or(6);
406        write!(
407            f,
408            "total mass = {} kg @  {}  {:?}",
409            format!("{:.*}", mass_prec, self.mass.total_mass_kg()),
410            format!("{:.*x}", orbit_prec, self.orbit),
411            self.mode,
412        )
413    }
414}
415
416#[allow(clippy::format_in_format_args)]
417impl fmt::UpperHex for Spacecraft {
418    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
419        let mass_prec = f.precision().unwrap_or(3);
420        let orbit_prec = f.precision().unwrap_or(6);
421        write!(
422            f,
423            "total mass = {} kg @  {}  {:?}",
424            format!("{:.*e}", mass_prec, self.mass.total_mass_kg()),
425            format!("{:.*X}", orbit_prec, self.orbit),
426            self.mode,
427        )
428    }
429}
430
431impl State for Spacecraft {
432    type Size = Const<9>;
433    type VecLength = Const<90>;
434
435    /// Copies the current state but sets the STM to identity
436    fn with_stm(mut self) -> Self {
437        self.enable_stm();
438        self
439    }
440
441    fn reset_stm(&mut self) {
442        self.stm = Some(OMatrix::<f64, Const<9>, Const<9>>::identity());
443    }
444
445    fn zeros() -> Self {
446        Self::default()
447    }
448
449    /// The vector is organized as such:
450    /// [X, Y, Z, Vx, Vy, Vz, Cr, Cd, Fuel mass, STM(9x9)]
451    fn to_vector(&self) -> OVector<f64, Const<90>> {
452        let mut vector = OVector::<f64, Const<90>>::zeros();
453        // Set the orbit state info
454        for (i, val) in self.orbit.radius_km.iter().enumerate() {
455            // Place the orbit state first, then skip three (Cr, Cd, Fuel), then copy orbit STM
456            vector[i] = *val;
457        }
458        for (i, val) in self.orbit.velocity_km_s.iter().enumerate() {
459            // Place the orbit state first, then skip three (Cr, Cd, Fuel), then copy orbit STM
460            vector[i + 3] = *val;
461        }
462        // Set the spacecraft parameters
463        vector[6] = self.srp.coeff_reflectivity;
464        vector[7] = self.drag.coeff_drag;
465        vector[8] = self.mass.prop_mass_kg;
466        // Add the STM to the vector
467        if let Some(stm) = self.stm {
468            for (idx, stm_val) in stm.as_slice().iter().enumerate() {
469                vector[idx + Self::Size::dim()] = *stm_val;
470            }
471        }
472        vector
473    }
474
475    /// Vector is expected to be organized as such:
476    /// [X, Y, Z, Vx, Vy, Vz, Cr, Cd, Fuel mass, STM(9x9)]
477    fn set(&mut self, epoch: Epoch, vector: &OVector<f64, Const<90>>) {
478        let sc_state =
479            OVector::<f64, Self::Size>::from_column_slice(&vector.as_slice()[..Self::Size::dim()]);
480
481        if self.stm.is_some() {
482            let sc_full_stm = OMatrix::<f64, Self::Size, Self::Size>::from_column_slice(
483                &vector.as_slice()[Self::Size::dim()..],
484            );
485
486            self.stm = Some(sc_full_stm);
487        }
488
489        let radius_km = sc_state.fixed_rows::<3>(0).into_owned();
490        let vel_km_s = sc_state.fixed_rows::<3>(3).into_owned();
491        self.orbit.epoch = epoch;
492        self.orbit.radius_km = radius_km;
493        self.orbit.velocity_km_s = vel_km_s;
494        self.srp.coeff_reflectivity = sc_state[6].clamp(0.0, 2.0);
495        self.drag.coeff_drag = sc_state[7];
496        self.mass.prop_mass_kg = sc_state[8];
497    }
498
499    /// diag(STM) = [X,Y,Z,Vx,Vy,Vz,Cr,Cd,Fuel]
500    /// WARNING: Currently the STM assumes that the prop mass is constant at ALL TIMES!
501    fn stm(&self) -> Result<OMatrix<f64, Self::Size, Self::Size>, DynamicsError> {
502        match self.stm {
503            Some(stm) => Ok(stm),
504            None => Err(DynamicsError::StateTransitionMatrixUnset),
505        }
506    }
507
508    fn epoch(&self) -> Epoch {
509        self.orbit.epoch
510    }
511
512    fn set_epoch(&mut self, epoch: Epoch) {
513        self.orbit.epoch = epoch
514    }
515
516    fn add(self, other: OVector<f64, Self::Size>) -> Self {
517        self + other
518    }
519
520    fn value(&self, param: StateParameter) -> Result<f64, StateError> {
521        match param {
522            StateParameter::Cd() => Ok(self.drag.coeff_drag),
523            StateParameter::Cr() => Ok(self.srp.coeff_reflectivity),
524            StateParameter::DryMass() => Ok(self.mass.dry_mass_kg),
525            StateParameter::PropMass() => Ok(self.mass.prop_mass_kg),
526            StateParameter::TotalMass() => Ok(self.mass.total_mass_kg()),
527            StateParameter::Isp() => match self.thruster {
528                Some(thruster) => Ok(thruster.isp_s),
529                None => Err(StateError::NoThrusterAvail),
530            },
531            StateParameter::ThrustX() => self
532                .thrust_direction()
533                .map(|direction| direction[0])
534                .ok_or(StateError::Unavailable { param }),
535            StateParameter::ThrustY() => self
536                .thrust_direction()
537                .map(|direction| direction[1])
538                .ok_or(StateError::Unavailable { param }),
539            StateParameter::ThrustZ() => self
540                .thrust_direction()
541                .map(|direction| direction[2])
542                .ok_or(StateError::Unavailable { param }),
543            StateParameter::ThrustInPlane(frame) => self
544                .thrust_angles_deg(frame)
545                .context(AstroPhysicsSnafu)
546                .context(StateAstroSnafu { param })?
547                .map(|(in_plane_deg, _)| in_plane_deg)
548                .ok_or(StateError::Unavailable { param }),
549            StateParameter::ThrustOutOfPlane(frame) => self
550                .thrust_angles_deg(frame)
551                .context(AstroPhysicsSnafu)
552                .context(StateAstroSnafu { param })?
553                .map(|(_, out_of_plane_deg)| out_of_plane_deg)
554                .ok_or(StateError::Unavailable { param }),
555            StateParameter::Thrust() => match self.thruster {
556                Some(thruster) => Ok(thruster.thrust_N),
557                None => Err(StateError::NoThrusterAvail),
558            },
559            StateParameter::GuidanceMode() => Ok(self.mode.into()),
560            StateParameter::Element(e) => e
561                .evaluate(self.orbit)
562                .context(AstroAnalysisSnafu)
563                .context(StateAstroSnafu { param }),
564            StateParameter::BdotR() => Ok(BPlane::new(self.orbit)
565                .context(StateAstroSnafu { param })?
566                .b_r_km
567                .real()),
568            StateParameter::BdotT() => Ok(BPlane::new(self.orbit)
569                .context(StateAstroSnafu { param })?
570                .b_t_km
571                .real()),
572            StateParameter::BLTOF() => Ok(BPlane::new(self.orbit)
573                .context(StateAstroSnafu { param })?
574                .ltof_s
575                .real()),
576            _ => Err(StateError::Unavailable { param }),
577        }
578    }
579
580    fn set_value(&mut self, param: StateParameter, val: f64) -> Result<(), StateError> {
581        match param {
582            StateParameter::Cd() => self.drag.coeff_drag = val,
583            StateParameter::Cr() => self.srp.coeff_reflectivity = val,
584            StateParameter::PropMass() => self.mass.prop_mass_kg = val,
585            StateParameter::DryMass() => self.mass.dry_mass_kg = val,
586            StateParameter::Isp() => match self.thruster {
587                Some(ref mut thruster) => thruster.isp_s = val,
588                None => return Err(StateError::NoThrusterAvail),
589            },
590            StateParameter::ThrustX() => {
591                let mut direction = self.thrust_direction();
592                if direction.is_none() {
593                    direction = Some(Vector3::zeros());
594                }
595                if let Some(mut direction) = direction {
596                    direction[0] = val;
597                    self.mut_thrust_direction(Some(direction));
598                }
599            }
600            StateParameter::ThrustY() => {
601                let mut direction = self.thrust_direction();
602                if direction.is_none() {
603                    direction = Some(Vector3::zeros());
604                }
605                if let Some(mut direction) = direction {
606                    direction[1] = val;
607                    self.mut_thrust_direction(Some(direction));
608                }
609            }
610            StateParameter::ThrustZ() => {
611                let mut direction = self.thrust_direction();
612                if direction.is_none() {
613                    direction = Some(Vector3::zeros());
614                }
615                if let Some(mut direction) = direction {
616                    direction[2] = val;
617                    self.mut_thrust_direction(Some(direction));
618                }
619            }
620            StateParameter::ThrustInPlane(_) | StateParameter::ThrustOutOfPlane(_) => {
621                return Err(StateError::ReadOnly { param })
622            }
623            StateParameter::Thrust() => match self.thruster {
624                Some(ref mut thruster) => thruster.thrust_N = val,
625                None => return Err(StateError::NoThrusterAvail),
626            },
627            StateParameter::Element(el) => {
628                match el {
629                    OrbitalElement::AoP => self
630                        .orbit
631                        .set_aop_deg(val)
632                        .context(AstroPhysicsSnafu)
633                        .context(StateAstroSnafu { param })?,
634                    OrbitalElement::Eccentricity => self
635                        .orbit
636                        .set_ecc(val)
637                        .context(AstroPhysicsSnafu)
638                        .context(StateAstroSnafu { param })?,
639                    OrbitalElement::Inclination => self
640                        .orbit
641                        .set_inc_deg(val)
642                        .context(AstroPhysicsSnafu)
643                        .context(StateAstroSnafu { param })?,
644                    OrbitalElement::RAAN => self
645                        .orbit
646                        .set_raan_deg(val)
647                        .context(AstroPhysicsSnafu)
648                        .context(StateAstroSnafu { param })?,
649                    OrbitalElement::SemiMajorAxis => self
650                        .orbit
651                        .set_sma_km(val)
652                        .context(AstroPhysicsSnafu)
653                        .context(StateAstroSnafu { param })?,
654                    OrbitalElement::TrueAnomaly => self
655                        .orbit
656                        .set_ta_deg(val)
657                        .context(AstroPhysicsSnafu)
658                        .context(StateAstroSnafu { param })?,
659                    OrbitalElement::X => self.orbit.radius_km.x = val,
660                    OrbitalElement::Y => self.orbit.radius_km.y = val,
661                    OrbitalElement::Z => self.orbit.radius_km.z = val,
662                    OrbitalElement::Rmag => {
663                        // Convert the position to spherical coordinates
664                        let (_, θ, φ) = cartesian_to_spherical(&self.orbit.radius_km);
665                        // Convert back to cartesian after setting the new range value
666                        self.orbit.radius_km = spherical_to_cartesian(val, θ, φ);
667                    }
668                    OrbitalElement::VX => self.orbit.velocity_km_s.x = val,
669                    OrbitalElement::VY => self.orbit.velocity_km_s.y = val,
670                    OrbitalElement::VZ => self.orbit.velocity_km_s.z = val,
671                    OrbitalElement::Vmag => {
672                        // Convert the velocity to spherical coordinates
673                        let (_, θ, φ) = cartesian_to_spherical(&self.orbit.velocity_km_s);
674                        // Convert back to cartesian after setting the new range value
675                        self.orbit.velocity_km_s = spherical_to_cartesian(val, θ, φ);
676                    }
677                    _ => return Err(StateError::ReadOnly { param }),
678                }
679            }
680            _ => return Err(StateError::ReadOnly { param }),
681        }
682        Ok(())
683    }
684
685    fn unset_stm(&mut self) {
686        self.stm = None;
687    }
688
689    fn orbit(&self) -> Orbit {
690        self.orbit
691    }
692
693    fn set_orbit(&mut self, orbit: Orbit) {
694        self.orbit = orbit;
695    }
696}
697
698impl Add<OVector<f64, Const<6>>> for Spacecraft {
699    type Output = Self;
700
701    /// Adds the provided state deviation to this orbit
702    fn add(mut self, other: OVector<f64, Const<6>>) -> Self {
703        let radius_km = other.fixed_rows::<3>(0).into_owned();
704        let vel_km_s = other.fixed_rows::<3>(3).into_owned();
705
706        self.orbit.radius_km += radius_km;
707        self.orbit.velocity_km_s += vel_km_s;
708
709        self
710    }
711}
712
713impl Add<OVector<f64, Const<9>>> for Spacecraft {
714    type Output = Self;
715
716    /// Adds the provided state deviation to this orbit
717    fn add(mut self, other: OVector<f64, Const<9>>) -> Self {
718        let radius_km = other.fixed_rows::<3>(0).into_owned();
719        let vel_km_s = other.fixed_rows::<3>(3).into_owned();
720
721        self.orbit.radius_km += radius_km;
722        self.orbit.velocity_km_s += vel_km_s;
723        self.srp.coeff_reflectivity = (self.srp.coeff_reflectivity + other[6]).clamp(0.0, 2.0);
724        self.drag.coeff_drag += other[7];
725        self.mass.prop_mass_kg += other[8];
726
727        self
728    }
729}
730
731impl<'a> Decode<'a> for Spacecraft {
732    fn decode<R: Reader<'a>>(decoder: &mut R) -> der::Result<Self> {
733        let orbit = decoder.decode()?;
734        let mass = decoder.decode()?;
735        let srp = decoder.decode()?;
736        let drag = decoder.decode()?;
737        let mode = decoder.decode()?;
738        // Decode the thruster last, checking the presence flag
739        let thruster = if decoder.decode::<bool>()? {
740            Some(decoder.decode()?)
741        } else {
742            None
743        };
744
745        Ok(Spacecraft {
746            orbit,
747            mass,
748            srp,
749            drag,
750            thruster,
751            mode,
752            thrust_direction: None,
753            stm: None,
754        })
755    }
756}
757
758impl Encode for Spacecraft {
759    fn encoded_len(&self) -> der::Result<der::Length> {
760        self.orbit.encoded_len()?
761            + self.mass.encoded_len()?
762            + self.srp.encoded_len()?
763            + self.drag.encoded_len()?
764            + self.mode.encoded_len()?
765            + self.thruster.is_some().encoded_len()?
766            + self.thruster.encoded_len()?
767    }
768
769    fn encode(&self, encoder: &mut impl der::Writer) -> der::Result<()> {
770        self.orbit.encode(encoder)?;
771        self.mass.encode(encoder)?;
772        self.srp.encode(encoder)?;
773        self.drag.encode(encoder)?;
774        self.mode.encode(encoder)?;
775        if let Some(thruster) = self.thruster {
776            true.encode(encoder)?;
777            thruster.encode(encoder)?;
778        } else {
779            false.encode(encoder)?
780        };
781
782        Ok(())
783    }
784}
785
786impl ConfigRepr for Spacecraft {}
787
788#[test]
789fn test_serde() {
790    use serde_yml;
791    use std::str::FromStr;
792
793    use anise::constants::frames::EARTH_J2000;
794
795    let orbit = Orbit::new(
796        -9042.862234,
797        18536.333069,
798        6999.957069,
799        -3.288789,
800        -2.226285,
801        1.646738,
802        Epoch::from_str("2018-09-15T00:15:53.098 UTC").unwrap(),
803        EARTH_J2000,
804    );
805
806    let sc = Spacecraft::new(orbit, 500.0, 159.0, 2.0, 2.0, 1.8, 2.2);
807
808    let serialized_sc = serde_yml::to_string(&sc).unwrap();
809    println!("{serialized_sc}");
810
811    let deser_sc: Spacecraft = serde_yml::from_str(&serialized_sc).unwrap();
812
813    assert_eq!(sc, deser_sc);
814
815    // Check that we can omit the thruster info entirely.
816    let s = r#"
817orbit:
818    radius_km:
819        - -9042.862234
820        - 18536.333069
821        - 6999.957069
822    velocity_km_s:
823        - -3.288789
824        - -2.226285
825        - 1.646738
826    epoch: 2018-09-15T00:15:53.098000000 UTC
827    frame:
828      ephemeris_id: 399
829      orientation_id: 1
830      force_inertial: false
831      mu_km3_s2: null
832      shape: null
833      frozen_epoch: null
834mass:
835    dry_mass_kg: 500.0
836    prop_mass_kg: 159.0
837    extra_mass_kg: 0.0
838srp:
839    area_m2: 2.0
840    coeff_reflectivity: 1.8
841drag:
842    area_m2: 2.0
843    coeff_drag: 2.2
844    "#;
845
846    let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap();
847    assert_eq!(sc, deser_sc);
848
849    // Check that we can specify a thruster info entirely.
850    let s = r#"
851orbit:
852    radius_km:
853    - -9042.862234
854    - 18536.333069
855    - 6999.957069
856    velocity_km_s:
857    - -3.288789
858    - -2.226285
859    - 1.646738
860    epoch: 2018-09-15T00:15:53.098000000 UTC
861    frame:
862        ephemeris_id: 399
863        orientation_id: 1
864        force_inertial: false
865        mu_km3_s2: null
866        shape: null
867        frozen_epoch: null
868mass:
869    dry_mass_kg: 500.0
870    prop_mass_kg: 159.0
871    extra_mass_kg: 0.0
872srp:
873    area_m2: 2.0
874    coeff_reflectivity: 1.8
875drag:
876    area_m2: 2.0
877    coeff_drag: 2.2
878thruster:
879    thrust_N: 1e-5
880    isp_s: 300.5
881    "#;
882
883    let mut sc_thruster = sc;
884    sc_thruster.thruster = Some(Thruster {
885        isp_s: 300.5,
886        thrust_N: 1e-5,
887    });
888    let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap();
889    assert_eq!(sc_thruster, deser_sc);
890
891    // Tests the minimum definition which will set all of the defaults too
892    let s = r#"
893orbit:
894    radius_km:
895    - -9042.862234
896    - 18536.333069
897    - 6999.957069
898    velocity_km_s:
899    - -3.288789
900    - -2.226285
901    - 1.646738
902    epoch: 2018-09-15T00:15:53.098000000 UTC
903    frame:
904        ephemeris_id: 399
905        orientation_id: 1
906        force_inertial: false
907        mu_km3_s2: null
908        shape: null
909        frozen_epoch: null
910mass:
911    dry_mass_kg: 500.0
912    prop_mass_kg: 159.0
913    extra_mass_kg: 0.0
914"#;
915
916    let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap();
917
918    let sc = Spacecraft::new(orbit, 500.0, 159.0, 0.0, 0.0, 1.8, 2.2);
919    assert_eq!(sc, deser_sc);
920}
nyx_space/cosmic/spacecraft.rs

nyx_space/cosmic/
spacecraft.rs
