1use 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 nalgebra::Vector3;
26use serde::{Deserialize, Serialize};
27use snafu::ResultExt;
28use typed_builder::TypedBuilder;
29
30use super::{AstroPhysicsSnafu, BPlane, State};
31use crate::cosmic::AstroAnalysisSnafu;
32use crate::dynamics::guidance::Thruster;
33use crate::dynamics::DynamicsError;
34use crate::errors::{StateAstroSnafu, StateError};
35use crate::io::ConfigRepr;
36use crate::linalg::{Const, DimName, OMatrix, OVector};
37use crate::md::StateParameter;
38use crate::time::Epoch;
39use crate::utils::{cartesian_to_spherical, spherical_to_cartesian};
40
41use std::default::Default;
42use std::fmt;
43use std::ops::Add;
44
45#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize, Default)]
46pub enum GuidanceMode {
47 #[default]
49 Coast,
50 Thrust,
52 Inhibit,
54}
55
56impl From<f64> for GuidanceMode {
57 fn from(value: f64) -> Self {
58 if value >= 1.0 {
59 Self::Thrust
60 } else if value < 0.0 {
61 Self::Inhibit
62 } else {
63 Self::Coast
64 }
65 }
66}
67
68impl From<GuidanceMode> for f64 {
69 fn from(mode: GuidanceMode) -> f64 {
70 match mode {
71 GuidanceMode::Coast => 0.0,
72 GuidanceMode::Thrust => 1.0,
73 GuidanceMode::Inhibit => -1.0,
74 }
75 }
76}
77
78#[derive(Clone, Copy, Debug, Serialize, Deserialize, TypedBuilder)]
82pub struct Spacecraft {
83 pub orbit: Orbit,
85 #[builder(default)]
87 pub mass: Mass,
88 #[builder(default)]
90 #[serde(default)]
91 pub srp: SRPData,
92 #[builder(default)]
93 #[serde(default)]
94 pub drag: DragData,
95 #[builder(default, setter(strip_option))]
96 pub thruster: Option<Thruster>,
97 #[builder(default)]
99 #[serde(default)]
100 pub mode: GuidanceMode,
101 #[builder(default, setter(strip_option))]
104 #[serde(skip)]
105 pub stm: Option<OMatrix<f64, Const<9>, Const<9>>>,
106}
107
108impl Default for Spacecraft {
109 fn default() -> Self {
110 Self {
111 orbit: Orbit::zero(EARTH_J2000),
112 mass: Mass::default(),
113 srp: SRPData::default(),
114 drag: DragData::default(),
115 thruster: None,
116 mode: GuidanceMode::default(),
117 stm: None,
118 }
119 }
120}
121
122impl From<Orbit> for Spacecraft {
123 fn from(orbit: Orbit) -> Self {
124 Self::builder().orbit(orbit).build()
125 }
126}
127
128impl Spacecraft {
129 pub fn new(
131 orbit: Orbit,
132 dry_mass_kg: f64,
133 prop_mass_kg: f64,
134 srp_area_m2: f64,
135 drag_area_m2: f64,
136 coeff_reflectivity: f64,
137 coeff_drag: f64,
138 ) -> Self {
139 Self {
140 orbit,
141 mass: Mass::from_dry_and_prop_masses(dry_mass_kg, prop_mass_kg),
142 srp: SRPData {
143 area_m2: srp_area_m2,
144 coeff_reflectivity,
145 },
146 drag: DragData {
147 area_m2: drag_area_m2,
148 coeff_drag,
149 },
150 ..Default::default()
151 }
152 }
153
154 pub fn from_thruster(
156 orbit: Orbit,
157 dry_mass_kg: f64,
158 prop_mass_kg: f64,
159 thruster: Thruster,
160 mode: GuidanceMode,
161 ) -> Self {
162 Self {
163 orbit,
164 mass: Mass::from_dry_and_prop_masses(dry_mass_kg, prop_mass_kg),
165 thruster: Some(thruster),
166 mode,
167 ..Default::default()
168 }
169 }
170
171 pub fn from_srp_defaults(orbit: Orbit, dry_mass_kg: f64, srp_area_m2: f64) -> Self {
173 Self {
174 orbit,
175 mass: Mass::from_dry_mass(dry_mass_kg),
176 srp: SRPData::from_area(srp_area_m2),
177 ..Default::default()
178 }
179 }
180
181 pub fn from_drag_defaults(orbit: Orbit, dry_mass_kg: f64, drag_area_m2: f64) -> Self {
183 Self {
184 orbit,
185 mass: Mass::from_dry_mass(dry_mass_kg),
186 drag: DragData::from_area(drag_area_m2),
187 ..Default::default()
188 }
189 }
190
191 pub fn with_dv_km_s(mut self, dv_km_s: Vector3<f64>) -> Self {
192 self.orbit.apply_dv_km_s(dv_km_s);
193 self
194 }
195
196 pub fn with_dry_mass(mut self, dry_mass_kg: f64) -> Self {
198 self.mass.dry_mass_kg = dry_mass_kg;
199 self
200 }
201
202 pub fn with_prop_mass(mut self, prop_mass_kg: f64) -> Self {
204 self.mass.prop_mass_kg = prop_mass_kg;
205 self
206 }
207
208 pub fn with_srp(mut self, srp_area_m2: f64, coeff_reflectivity: f64) -> Self {
210 self.srp = SRPData {
211 area_m2: srp_area_m2,
212 coeff_reflectivity,
213 };
214
215 self
216 }
217
218 pub fn with_srp_area(mut self, srp_area_m2: f64) -> Self {
220 self.srp.area_m2 = srp_area_m2;
221 self
222 }
223
224 pub fn with_cr(mut self, coeff_reflectivity: f64) -> Self {
226 self.srp.coeff_reflectivity = coeff_reflectivity;
227 self
228 }
229
230 pub fn with_drag(mut self, drag_area_m2: f64, coeff_drag: f64) -> Self {
232 self.drag = DragData {
233 area_m2: drag_area_m2,
234 coeff_drag,
235 };
236 self
237 }
238
239 pub fn with_drag_area(mut self, drag_area_m2: f64) -> Self {
241 self.drag.area_m2 = drag_area_m2;
242 self
243 }
244
245 pub fn with_cd(mut self, coeff_drag: f64) -> Self {
247 self.drag.coeff_drag = coeff_drag;
248 self
249 }
250
251 pub fn with_orbit(mut self, orbit: Orbit) -> Self {
253 self.orbit = orbit;
254 self
255 }
256
257 pub fn rss(&self, other: &Self) -> PhysicsResult<(f64, f64, f64)> {
259 let rss_p_km = self.orbit.rss_radius_km(&other.orbit)?;
260 let rss_v_km_s = self.orbit.rss_velocity_km_s(&other.orbit)?;
261 let rss_prop_kg = (self.mass.prop_mass_kg - other.mass.prop_mass_kg)
262 .powi(2)
263 .sqrt();
264
265 Ok((rss_p_km, rss_v_km_s, rss_prop_kg))
266 }
267
268 pub fn enable_stm(&mut self) {
270 self.stm = Some(OMatrix::<f64, Const<9>, Const<9>>::identity());
271 }
272
273 pub fn mass_kg(&self) -> f64 {
275 self.mass.total_mass_kg()
276 }
277
278 pub fn with_guidance_mode(mut self, mode: GuidanceMode) -> Self {
280 self.mode = mode;
281 self
282 }
283
284 pub fn mode(&self) -> GuidanceMode {
285 self.mode
286 }
287
288 pub fn mut_mode(&mut self, mode: GuidanceMode) {
289 self.mode = mode;
290 }
291}
292
293impl PartialEq for Spacecraft {
294 fn eq(&self, other: &Self) -> bool {
295 let mass_tol = 1e-6; self.orbit.eq_within(&other.orbit, 1e-9, 1e-12)
297 && (self.mass - other.mass).abs().total_mass_kg() < mass_tol
298 && self.srp == other.srp
299 && self.drag == other.drag
300 }
301}
302
303#[allow(clippy::format_in_format_args)]
304impl fmt::Display for Spacecraft {
305 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
306 let mass_prec = f.precision().unwrap_or(3);
307 let orbit_prec = f.precision().unwrap_or(6);
308 write!(
309 f,
310 "total mass = {} kg @ {} {:?}",
311 format!("{:.*}", mass_prec, self.mass.total_mass_kg()),
312 format!("{:.*}", orbit_prec, self.orbit),
313 self.mode,
314 )
315 }
316}
317
318#[allow(clippy::format_in_format_args)]
319impl fmt::LowerExp for Spacecraft {
320 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
321 let mass_prec = f.precision().unwrap_or(3);
322 let orbit_prec = f.precision().unwrap_or(6);
323 write!(
324 f,
325 "total mass = {} kg @ {} {:?}",
326 format!("{:.*e}", mass_prec, self.mass.total_mass_kg()),
327 format!("{:.*e}", orbit_prec, self.orbit),
328 self.mode,
329 )
330 }
331}
332
333#[allow(clippy::format_in_format_args)]
334impl fmt::LowerHex for Spacecraft {
335 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
336 let mass_prec = f.precision().unwrap_or(3);
337 let orbit_prec = f.precision().unwrap_or(6);
338 write!(
339 f,
340 "total mass = {} kg @ {} {:?}",
341 format!("{:.*}", mass_prec, self.mass.total_mass_kg()),
342 format!("{:.*x}", orbit_prec, self.orbit),
343 self.mode,
344 )
345 }
346}
347
348#[allow(clippy::format_in_format_args)]
349impl fmt::UpperHex for Spacecraft {
350 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
351 let mass_prec = f.precision().unwrap_or(3);
352 let orbit_prec = f.precision().unwrap_or(6);
353 write!(
354 f,
355 "total mass = {} kg @ {} {:?}",
356 format!("{:.*e}", mass_prec, self.mass.total_mass_kg()),
357 format!("{:.*X}", orbit_prec, self.orbit),
358 self.mode,
359 )
360 }
361}
362
363impl State for Spacecraft {
364 type Size = Const<9>;
365 type VecLength = Const<90>;
366
367 fn with_stm(mut self) -> Self {
369 self.enable_stm();
370 self
371 }
372
373 fn reset_stm(&mut self) {
374 self.stm = Some(OMatrix::<f64, Const<9>, Const<9>>::identity());
375 }
376
377 fn zeros() -> Self {
378 Self::default()
379 }
380
381 fn to_vector(&self) -> OVector<f64, Const<90>> {
384 let mut vector = OVector::<f64, Const<90>>::zeros();
385 for (i, val) in self.orbit.radius_km.iter().enumerate() {
387 vector[i] = *val;
389 }
390 for (i, val) in self.orbit.velocity_km_s.iter().enumerate() {
391 vector[i + 3] = *val;
393 }
394 vector[6] = self.srp.coeff_reflectivity;
396 vector[7] = self.drag.coeff_drag;
397 vector[8] = self.mass.prop_mass_kg;
398 if let Some(stm) = self.stm {
400 for (idx, stm_val) in stm.as_slice().iter().enumerate() {
401 vector[idx + Self::Size::dim()] = *stm_val;
402 }
403 }
404 vector
405 }
406
407 fn set(&mut self, epoch: Epoch, vector: &OVector<f64, Const<90>>) {
410 let sc_state =
411 OVector::<f64, Self::Size>::from_column_slice(&vector.as_slice()[..Self::Size::dim()]);
412
413 if self.stm.is_some() {
414 let sc_full_stm = OMatrix::<f64, Self::Size, Self::Size>::from_column_slice(
415 &vector.as_slice()[Self::Size::dim()..],
416 );
417
418 self.stm = Some(sc_full_stm);
419 }
420
421 let radius_km = sc_state.fixed_rows::<3>(0).into_owned();
422 let vel_km_s = sc_state.fixed_rows::<3>(3).into_owned();
423 self.orbit.epoch = epoch;
424 self.orbit.radius_km = radius_km;
425 self.orbit.velocity_km_s = vel_km_s;
426 self.srp.coeff_reflectivity = sc_state[6].clamp(0.0, 2.0);
427 self.drag.coeff_drag = sc_state[7];
428 self.mass.prop_mass_kg = sc_state[8];
429 }
430
431 fn stm(&self) -> Result<OMatrix<f64, Self::Size, Self::Size>, DynamicsError> {
434 match self.stm {
435 Some(stm) => Ok(stm),
436 None => Err(DynamicsError::StateTransitionMatrixUnset),
437 }
438 }
439
440 fn epoch(&self) -> Epoch {
441 self.orbit.epoch
442 }
443
444 fn set_epoch(&mut self, epoch: Epoch) {
445 self.orbit.epoch = epoch
446 }
447
448 fn add(self, other: OVector<f64, Self::Size>) -> Self {
449 self + other
450 }
451
452 fn value(&self, param: StateParameter) -> Result<f64, StateError> {
453 match param {
454 StateParameter::Cd => Ok(self.drag.coeff_drag),
455 StateParameter::Cr => Ok(self.srp.coeff_reflectivity),
456 StateParameter::DryMass => Ok(self.mass.dry_mass_kg),
457 StateParameter::PropMass => Ok(self.mass.prop_mass_kg),
458 StateParameter::TotalMass => Ok(self.mass.total_mass_kg()),
459 StateParameter::Isp => match self.thruster {
460 Some(thruster) => Ok(thruster.isp_s),
461 None => Err(StateError::NoThrusterAvail),
462 },
463 StateParameter::Thrust => match self.thruster {
464 Some(thruster) => Ok(thruster.thrust_N),
465 None => Err(StateError::NoThrusterAvail),
466 },
467 StateParameter::GuidanceMode => Ok(self.mode.into()),
468 StateParameter::Element(e) => e
469 .evaluate(self.orbit)
470 .context(AstroAnalysisSnafu)
471 .context(StateAstroSnafu { param }),
472 StateParameter::BdotR => Ok(BPlane::new(self.orbit)
473 .context(StateAstroSnafu { param })?
474 .b_r_km
475 .real()),
476 StateParameter::BdotT => Ok(BPlane::new(self.orbit)
477 .context(StateAstroSnafu { param })?
478 .b_t_km
479 .real()),
480 StateParameter::BLTOF => Ok(BPlane::new(self.orbit)
481 .context(StateAstroSnafu { param })?
482 .ltof_s
483 .real()),
484 _ => Err(StateError::Unavailable { param }),
485 }
486 }
487
488 fn set_value(&mut self, param: StateParameter, val: f64) -> Result<(), StateError> {
489 match param {
490 StateParameter::Cd => self.drag.coeff_drag = val,
491 StateParameter::Cr => self.srp.coeff_reflectivity = val,
492 StateParameter::PropMass => self.mass.prop_mass_kg = val,
493 StateParameter::DryMass => self.mass.dry_mass_kg = val,
494 StateParameter::Isp => match self.thruster {
495 Some(ref mut thruster) => thruster.isp_s = val,
496 None => return Err(StateError::NoThrusterAvail),
497 },
498 StateParameter::Thrust => match self.thruster {
499 Some(ref mut thruster) => thruster.thrust_N = val,
500 None => return Err(StateError::NoThrusterAvail),
501 },
502 StateParameter::Element(el) => {
503 match el {
504 OrbitalElement::AoP => self
505 .orbit
506 .set_aop_deg(val)
507 .context(AstroPhysicsSnafu)
508 .context(StateAstroSnafu { param })?,
509 OrbitalElement::Eccentricity => self
510 .orbit
511 .set_ecc(val)
512 .context(AstroPhysicsSnafu)
513 .context(StateAstroSnafu { param })?,
514 OrbitalElement::Inclination => self
515 .orbit
516 .set_inc_deg(val)
517 .context(AstroPhysicsSnafu)
518 .context(StateAstroSnafu { param })?,
519 OrbitalElement::RAAN => self
520 .orbit
521 .set_raan_deg(val)
522 .context(AstroPhysicsSnafu)
523 .context(StateAstroSnafu { param })?,
524 OrbitalElement::SemiMajorAxis => self
525 .orbit
526 .set_sma_km(val)
527 .context(AstroPhysicsSnafu)
528 .context(StateAstroSnafu { param })?,
529 OrbitalElement::TrueAnomaly => self
530 .orbit
531 .set_ta_deg(val)
532 .context(AstroPhysicsSnafu)
533 .context(StateAstroSnafu { param })?,
534 OrbitalElement::X => self.orbit.radius_km.x = val,
535 OrbitalElement::Y => self.orbit.radius_km.y = val,
536 OrbitalElement::Z => self.orbit.radius_km.z = val,
537 OrbitalElement::Rmag => {
538 let (_, θ, φ) = cartesian_to_spherical(&self.orbit.radius_km);
540 self.orbit.radius_km = spherical_to_cartesian(val, θ, φ);
542 }
543 OrbitalElement::VX => self.orbit.velocity_km_s.x = val,
544 OrbitalElement::VY => self.orbit.velocity_km_s.y = val,
545 OrbitalElement::VZ => self.orbit.velocity_km_s.z = val,
546 OrbitalElement::Vmag => {
547 let (_, θ, φ) = cartesian_to_spherical(&self.orbit.velocity_km_s);
549 self.orbit.velocity_km_s = spherical_to_cartesian(val, θ, φ);
551 }
552 _ => return Err(StateError::ReadOnly { param }),
553 }
554 }
555 _ => return Err(StateError::ReadOnly { param }),
556 }
557 Ok(())
558 }
559
560 fn unset_stm(&mut self) {
561 self.stm = None;
562 }
563
564 fn orbit(&self) -> Orbit {
565 self.orbit
566 }
567
568 fn set_orbit(&mut self, orbit: Orbit) {
569 self.orbit = orbit;
570 }
571}
572
573impl Add<OVector<f64, Const<6>>> for Spacecraft {
574 type Output = Self;
575
576 fn add(mut self, other: OVector<f64, Const<6>>) -> Self {
578 let radius_km = other.fixed_rows::<3>(0).into_owned();
579 let vel_km_s = other.fixed_rows::<3>(3).into_owned();
580
581 self.orbit.radius_km += radius_km;
582 self.orbit.velocity_km_s += vel_km_s;
583
584 self
585 }
586}
587
588impl Add<OVector<f64, Const<9>>> for Spacecraft {
589 type Output = Self;
590
591 fn add(mut self, other: OVector<f64, Const<9>>) -> Self {
593 let radius_km = other.fixed_rows::<3>(0).into_owned();
594 let vel_km_s = other.fixed_rows::<3>(3).into_owned();
595
596 self.orbit.radius_km += radius_km;
597 self.orbit.velocity_km_s += vel_km_s;
598 self.srp.coeff_reflectivity = (self.srp.coeff_reflectivity + other[6]).clamp(0.0, 2.0);
599 self.drag.coeff_drag += other[7];
600 self.mass.prop_mass_kg += other[8];
601
602 self
603 }
604}
605
606impl ConfigRepr for Spacecraft {}
607
608#[test]
609fn test_serde() {
610 use serde_yml;
611 use std::str::FromStr;
612
613 use anise::constants::frames::EARTH_J2000;
614
615 let orbit = Orbit::new(
616 -9042.862234,
617 18536.333069,
618 6999.957069,
619 -3.288789,
620 -2.226285,
621 1.646738,
622 Epoch::from_str("2018-09-15T00:15:53.098 UTC").unwrap(),
623 EARTH_J2000,
624 );
625
626 let sc = Spacecraft::new(orbit, 500.0, 159.0, 2.0, 2.0, 1.8, 2.2);
627
628 let serialized_sc = serde_yml::to_string(&sc).unwrap();
629 println!("{serialized_sc}");
630
631 let deser_sc: Spacecraft = serde_yml::from_str(&serialized_sc).unwrap();
632
633 assert_eq!(sc, deser_sc);
634
635 let s = r#"
637orbit:
638 radius_km:
639 - -9042.862234
640 - 18536.333069
641 - 6999.957069
642 velocity_km_s:
643 - -3.288789
644 - -2.226285
645 - 1.646738
646 epoch: 2018-09-15T00:15:53.098000000 UTC
647 frame:
648 ephemeris_id: 399
649 orientation_id: 1
650 mu_km3_s2: null
651 shape: null
652mass:
653 dry_mass_kg: 500.0
654 prop_mass_kg: 159.0
655 extra_mass_kg: 0.0
656srp:
657 area_m2: 2.0
658 coeff_reflectivity: 1.8
659drag:
660 area_m2: 2.0
661 coeff_drag: 2.2
662 "#;
663
664 let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap();
665 assert_eq!(sc, deser_sc);
666
667 let s = r#"
669orbit:
670 radius_km:
671 - -9042.862234
672 - 18536.333069
673 - 6999.957069
674 velocity_km_s:
675 - -3.288789
676 - -2.226285
677 - 1.646738
678 epoch: 2018-09-15T00:15:53.098000000 UTC
679 frame:
680 ephemeris_id: 399
681 orientation_id: 1
682 mu_km3_s2: null
683 shape: null
684mass:
685 dry_mass_kg: 500.0
686 prop_mass_kg: 159.0
687 extra_mass_kg: 0.0
688srp:
689 area_m2: 2.0
690 coeff_reflectivity: 1.8
691drag:
692 area_m2: 2.0
693 coeff_drag: 2.2
694thruster:
695 thrust_N: 1e-5
696 isp_s: 300.5
697 "#;
698
699 let mut sc_thruster = sc;
700 sc_thruster.thruster = Some(Thruster {
701 isp_s: 300.5,
702 thrust_N: 1e-5,
703 });
704 let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap();
705 assert_eq!(sc_thruster, deser_sc);
706
707 let s = r#"
709orbit:
710 radius_km:
711 - -9042.862234
712 - 18536.333069
713 - 6999.957069
714 velocity_km_s:
715 - -3.288789
716 - -2.226285
717 - 1.646738
718 epoch: 2018-09-15T00:15:53.098000000 UTC
719 frame:
720 ephemeris_id: 399
721 orientation_id: 1
722 mu_km3_s2: null
723 shape: null
724mass:
725 dry_mass_kg: 500.0
726 prop_mass_kg: 159.0
727 extra_mass_kg: 0.0
728"#;
729
730 let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap();
731
732 let sc = Spacecraft::new(orbit, 500.0, 159.0, 0.0, 0.0, 1.8, 2.2);
733 assert_eq!(sc, deser_sc);
734}