diff --git a/Cargo.toml b/Cargo.toml index 7eb0b6aa..a5135acd 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -32,65 +32,56 @@ exclude = [ [badges] maintenance = { status = "actively-developed" } -gitlab = { repository = "nyx-space/nyx", branch = "master" } +github = { repository = "nyx-space/nyx", branch = "master" } [dependencies] nalgebra = "0.33" log = "0.4" hifitime = "4.0.0" -anise = "0.5.0" +anise = "0.5.2" flate2 = { version = "1.0", features = [ "rust_backend", ], default-features = false } serde = "1.0" serde_derive = "1.0" -csv = "1" hyperdual = "1.3.0" -bytes = "1.0" rand = "0.8" rand_distr = "0.4" regex = "1.5" rayon = "1.6" -lazy_static = "1.4.0" approx = "0.5" rand_pcg = "0.3" -indicatif = { version = "0.17", features = ["rayon"] } +indicatif = { version = "0.17", features = ["rayon"], default-features = false } rstats = "2.0.1" -parquet = { version = "53.0.0", default-features = false, features = [ +parquet = { version = "54.0.0", default-features = false, features = [ "arrow", "zstd", ] } -arrow = "53.0.0" -shadow-rs = { version = "0.36.0", default-features = false } +arrow = "54.0.0" +shadow-rs = { version = "0.37.0", default-features = false } serde_yml = "0.0.12" whoami = "1.3.0" either = { version = "1.8.1", features = ["serde"] } num = "0.4.0" enum-iterator = "2.0.0" -getrandom = { version = "0.2", features = ["js"] } typed-builder = "0.20.0" snafu = { version = "0.8.3", features = ["backtrace"] } serde_dhall = "0.12" -indexmap = {version = "2.6.0", features = ["serde"]} +indexmap = { version = "2.6.0", features = ["serde"] } [dev-dependencies] -polars = { version = "0.43.1", features = ["parquet"] } +polars = { version = "0.45.1", features = ["parquet"] } rstest = "0.23.0" pretty_env_logger = "0.5" toml = "0.8.14" [build-dependencies] -shadow-rs = "0.36.0" +shadow-rs = "0.37.0" -[features] -default = [] -# python = ["pyo3", "pyo3-log", "hifitime/python", "numpy", "pythonize"] -python = [] - -[lib] -crate-type = ["cdylib", "rlib"] -name = "nyx_space" +# Uncomment to speed up local builds +# [profile.dev.package."*"] +# opt-level = 3 [target.x86_64-unknown-linux-gnu] # For flamegraph -- https://github.com/flamegraph-rs/flamegraph diff --git a/README.md b/README.md index b5b7a0bc..d06dcc43 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ [**Empowering flight dynamics engineers with open-source software**][website] -Nyx is revolutionizing the field of flight dynamics engineering as a powerful, open-source tool for mission design and orbit determination. From trajectory optimization to orbit estimation, Nyx is built for speed, automation, and scalability. It dramatically reduces simulation time compared to commercial products, and integrates seamlessly into automated workflows across various platforms. +Nyx is revolutionizing the field of flight dynamics engineering as a powerful, open-source tool for mission design and orbit determination. From trajectory optimization to orbit estimation, Nyx is built for speed, automation, and scalability. **Nyx has proven mission-critical reliability, already contributing to the success of three lunar missions.** @@ -13,6 +13,28 @@ Nyx is revolutionizing the field of flight dynamics engineering as a powerful, o [![nyx-space on docs.rs][docsrs-image]][docsrs] [![codecov](https://codecov.io/gh/nyx-space/nyx/graph/badge.svg?token=gEiAvwzwh5)](https://codecov.io/gh/nyx-space/nyx) +# Showcase + +[The website has the latest use cases][showcase] + +## GEO Low Thrust Orbit Raising & StationKeeping + +[Click for description](https://nyxspace.com/nyxspace/showcase/03_geo_analysis/?utm_source=readme-showcase) + +[![RAAN, AOP, INC over time](./examples/03_geo_analysis/plots/raise-traj-3d.png)](https://nyxspace.com/nyxspace/showcase/03_geo_analysis/?utm_source=readme-showcase) + +## James Webb Space Telescope Monte Carlo Simulation + +[Click for description](https://nyxspace.com/nyxspace/showcase/02_jwst_covar_monte_carlo/?utm_source=readme-showcase) + +[![RAAN, AOP, INC over time](./examples/02_jwst_covar_monte_carlo/plots/jwst_mc_inc_deg.png)](https://nyxspace.com/nyxspace/showcase/02_jwst_covar_monte_carlo/?utm_source=readme-showcase) + +## Orbit Determination of the Lunar Reconnaissance Orbiter + +[Click for description](https://nyxspace.com/nyxspace/showcase/04_lro_od/?utm_source=readme-showcase) + +[![RAAN, AOP, INC over time](./examples/04_lro_od/plots/doppler-resid.png)](https://nyxspace.com/nyxspace/showcase/04_lro_od/?utm_source=readme-showcase) + # Documentation The documentation is currently being updated. If you have specific use cases you would like to see documented, please [open a Github issue](https://github.com/nyx-space/nyx/issues/new?assignees=&labels=Documentation&projects=&template=documentation.md&title=) or [use the contact form][contact] @@ -49,8 +71,10 @@ Nyx is provided under the [AGPLv3 License](./LICENSE). By using this software, y [contact]: https://7ug5imdtt8v.typeform.com/to/neFvVW3p [nyxspace-image]: https://img.shields.io/badge/Nyx_Space-Website-orange [website]: https://nyxspace.com/?utm_source=readme +[showcase]: https://nyxspace.com/nyxspace/showcase/?utm_source=readme # Author information + > Chris Rabotin is a GNC and flight dynamics engineer with a heavy background in software. I currently work for Rocket Lab USA as the lead flight dynamics engineer on both Blue Ghost lunar lander missions. -- Find me on [LinkedIn](https://www.linkedin.com/in/chrisrabotin/). diff --git a/build.rs b/build.rs index 4a0dfc45..c5150851 100644 --- a/build.rs +++ b/build.rs @@ -1,3 +1,7 @@ -fn main() -> shadow_rs::SdResult<()> { - shadow_rs::new() +use shadow_rs::ShadowBuilder; + +fn main() { + ShadowBuilder::builder() + .build() + .expect("shadow init for nyx_space failed"); } diff --git a/data/tests/config/spacecraft.yaml b/data/tests/config/spacecraft.yaml index 2627d4f5..d4ac8b45 100644 --- a/data/tests/config/spacecraft.yaml +++ b/data/tests/config/spacecraft.yaml @@ -8,13 +8,15 @@ orbit: frame: EME2000 epoch: 2018-09-15T00:15:53.098 UTC srp: - cr: 1.0 + coeff_reflectivity:1.0 area_m2: 2.0 drag: - cd: 2.2 + coeff_drag: 2.2 area_m2: 0.95 -dry_mass_kg: 50.0 -fuel_mass_kg: 50.0 +mass: + dry_mass_kg: 50.0 + prop_mass_kg: 50.0 + extra_mass_kg: 0.0 thruster: thrust_N: 1e-5 isp_s: 300.0 diff --git a/examples/01_orbit_prop/main.rs b/examples/01_orbit_prop/main.rs index 451f11c2..d2f9739c 100644 --- a/examples/01_orbit_prop/main.rs +++ b/examples/01_orbit_prop/main.rs @@ -13,14 +13,17 @@ use anise::{ use hifitime::{Epoch, Unit}; use log::warn; use nyx::{ - cosmic::{MetaAlmanac, Orbit, SrpConfig}, + cosmic::{Mass, MetaAlmanac, Orbit, SRPData}, dynamics::{Harmonics, OrbitalDynamics, SolarPressure, SpacecraftDynamics}, io::{gravity::HarmonicsMem, ExportCfg}, od::GroundStation, propagators::Propagator, Spacecraft, State, }; -use polars::{df, series::ChunkCompare}; +use polars::{ + frame::column::ScalarColumn, + prelude::{df, AnyValue, ChunkCompareIneq, Column, DataType, Scalar}, +}; use std::{error::Error, sync::Arc}; @@ -91,10 +94,10 @@ fn main() -> Result<(), Box> { // Let's build a cubesat sized spacecraft, with an SRP area of 10 cm^2 and a mass of 9.6 kg. let sc = Spacecraft::builder() .orbit(orbit) - .dry_mass_kg(9.60) - .srp(SrpConfig { + .mass(Mass::from_dry_mass(9.60)) + .srp(SRPData { area_m2: 10e-4, - cr: 1.1, + coeff_reflectivity: 1.1, }) .build(); println!("{sc:x}"); @@ -274,7 +277,13 @@ fn main() -> Result<(), Box> { )?; // Finally, let's see when the spacecraft is visible, assuming 15 degrees minimum elevation. - let mask = aer_df.column("elevation (deg)")?.gt(15.0)?; + let mask = aer_df + .column("elevation (deg)")? + .gt(&Column::Scalar(ScalarColumn::new( + "elevation mask (deg)".into(), + Scalar::new(DataType::Float64, AnyValue::Float64(15.0)), + offset_s.len(), + )))?; let cubesat_visible = aer_df.filter(&mask)?; println!("{cubesat_visible}"); diff --git a/examples/02_jwst_covar_monte_carlo/main.rs b/examples/02_jwst_covar_monte_carlo/main.rs index a5cbfc3f..1abe25e8 100644 --- a/examples/02_jwst_covar_monte_carlo/main.rs +++ b/examples/02_jwst_covar_monte_carlo/main.rs @@ -12,7 +12,7 @@ use anise::{ }; use hifitime::{TimeUnits, Unit}; use nyx::{ - cosmic::{eclipse::EclipseLocator, Frame, MetaAlmanac, SrpConfig}, + cosmic::{eclipse::EclipseLocator, Frame, Mass, MetaAlmanac, SRPData}, dynamics::{guidance::LocalFrame, OrbitalDynamics, SolarPressure, SpacecraftDynamics}, io::ExportCfg, mc::MonteCarlo, @@ -62,11 +62,11 @@ fn main() -> Result<(), Box> { // SRP Coefficient of reflectivity assumed to be that of Kapton, i.e. 2 - 0.44 = 1.56, table 1 from https://amostech.com/TechnicalPapers/2018/Poster/Bengtson.pdf let jwst = Spacecraft::builder() .orbit(jwst_orbit) - .srp(SrpConfig { + .srp(SRPData { area_m2: 21.197 * 14.162, - cr: 1.56, + coeff_reflectivity: 1.56, }) - .dry_mass_kg(6200.0) + .mass(Mass::from_dry_mass(6200.0)) .build(); // Build up the spacecraft uncertainty builder. diff --git a/examples/03_geo_analysis/README.md b/examples/03_geo_analysis/README.md index 6bef6fca..9703e924 100644 --- a/examples/03_geo_analysis/README.md +++ b/examples/03_geo_analysis/README.md @@ -91,7 +91,7 @@ To build the following plots, use the `plot_3d_traj.py` script and the `plot_orb ![Orbital elements during orbit raise](./plots/raise-keplerian-oe.png) -In the two follow plots, the colors correspond to the remaining fuel mass, thereby showing the fuel depletion over the orbit raise. +In the two follow plots, the colors correspond to the remaining prop mass, thereby showing the prop depletion over the orbit raise. ![3D traj raise](./plots/raise-traj-3d.png) @@ -112,7 +112,7 @@ To run the [station keeping Monte Carlo](./stationkeeping.rs) example, just exec RUST_LOG=info cargo run --example 03_geo_sk --release ``` -Over a two week period, this two-ton spacecraft would need roughly 0.8 kg of fuel (if using the _NEXT-STEP_ engine, cf. the comments in the drift analysis code) +/- 0.1 kg for station keeping. +Over a two week period, this two-ton spacecraft would need roughly 0.8 kg of prop (if using the _NEXT-STEP_ engine, cf. the comments in the drift analysis code) +/- 0.1 kg for station keeping. ![Fuel mass](./plots/sk-fuel-mass.png) @@ -123,4 +123,4 @@ The inclination plot shows when the guidance law turns on, and shows that we mai ### Further analysis -Additional analysis would run this Monte Carlo for longer and with many more spacecraft (upward of 100), and crucially ensure that the Ruggiero guidance law bounds correspond to the GEO box. Subsequently, one should implement the Q-Law guidance law for more fuel economy. Finally, the analysis should also include a variation on the tightness of the box, especially if the vehicle is equipped with a variable thrust engine as one may wish to drift less out of the SK box and keep the engine at a lower thrust level, or vice versa. \ No newline at end of file +Additional analysis would run this Monte Carlo for longer and with many more spacecraft (upward of 100), and crucially ensure that the Ruggiero guidance law bounds correspond to the GEO box. Subsequently, one should implement the Q-Law guidance law for more prop economy. Finally, the analysis should also include a variation on the tightness of the box, especially if the vehicle is equipped with a variable thrust engine as one may wish to drift less out of the SK box and keep the engine at a lower thrust level, or vice versa. \ No newline at end of file diff --git a/examples/03_geo_analysis/drift.rs b/examples/03_geo_analysis/drift.rs index 82fa541f..c75888bb 100644 --- a/examples/03_geo_analysis/drift.rs +++ b/examples/03_geo_analysis/drift.rs @@ -12,7 +12,7 @@ use anise::{ }; use hifitime::{Epoch, Unit}; use nyx::{ - cosmic::{eclipse::EclipseLocator, MetaAlmanac, Orbit, SrpConfig}, + cosmic::{eclipse::EclipseLocator, Mass, MetaAlmanac, Orbit, SRPData}, dynamics::{Harmonics, OrbitalDynamics, SolarPressure, SpacecraftDynamics}, io::{gravity::HarmonicsMem, ExportCfg}, propagators::Propagator, @@ -61,10 +61,10 @@ fn main() -> Result<(), Box> { // Let's build a cubesat sized spacecraft, with an SRP area of 10 cm^2 and a mass of 9.6 kg. let sc = Spacecraft::builder() .orbit(orbit) - .dry_mass_kg(9.60) - .srp(SrpConfig { + .mass(Mass::from_dry_mass(9.60)) + .srp(SRPData { area_m2: 10e-4, - cr: 1.1, + coeff_reflectivity: 1.1, }) .build(); println!("{sc:x}"); diff --git a/examples/03_geo_analysis/plot_3d_traj.py b/examples/03_geo_analysis/plot_3d_traj.py index c289a803..69d58d03 100644 --- a/examples/03_geo_analysis/plot_3d_traj.py +++ b/examples/03_geo_analysis/plot_3d_traj.py @@ -37,8 +37,8 @@ def build_sphere(size, num_points=500, opacity=1.0): def plot_traj( df: pl.DataFrame, - colored_by="fuel_mass (kg)", - color_descr="fuel mass (kg)", + colored_by="prop_mass (kg)", + color_descr="prop mass (kg)", scale=1.0 ): """ diff --git a/examples/03_geo_analysis/plot_sk_mc.py b/examples/03_geo_analysis/plot_sk_mc.py index 29420129..4531e0ff 100644 --- a/examples/03_geo_analysis/plot_sk_mc.py +++ b/examples/03_geo_analysis/plot_sk_mc.py @@ -18,7 +18,7 @@ "ta (deg)", "aol (deg)", "tlong (deg)", - "fuel_mass (kg)", + "prop_mass (kg)", ] for col in columns: diff --git a/examples/03_geo_analysis/raise.rs b/examples/03_geo_analysis/raise.rs index d1ad84f4..4e7e956e 100644 --- a/examples/03_geo_analysis/raise.rs +++ b/examples/03_geo_analysis/raise.rs @@ -12,7 +12,7 @@ use anise::{ }; use hifitime::{Epoch, TimeUnits, Unit}; use nyx::{ - cosmic::{eclipse::EclipseLocator, GuidanceMode, MetaAlmanac, Orbit, SrpConfig}, + cosmic::{eclipse::EclipseLocator, GuidanceMode, Mass, MetaAlmanac, Orbit, SRPData}, dynamics::{ guidance::{GuidanceLaw, Ruggiero, Thruster}, Harmonics, OrbitalDynamics, SolarPressure, SpacecraftDynamics, @@ -51,9 +51,8 @@ fn main() -> Result<(), Box> { let sc = Spacecraft::builder() .orbit(orbit) - .dry_mass_kg(1000.0) // 1000 kg of dry mass - .fuel_mass_kg(1000.0) // 1000 kg of fuel, totalling 2.0 tons - .srp(SrpConfig::from_area(3.0 * 6.0)) // Assuming 1 kW/m^2 or 18 kW, giving a margin of 4.35 kW for on-propulsion consumption + .mass(Mass::from_dry_and_prop_masses(1000.0, 1000.0)) // 1000 kg of dry mass and prop, totalling 2.0 tons + .srp(SRPData::from_area(3.0 * 6.0)) // Assuming 1 kW/m^2 or 18 kW, giving a margin of 4.35 kW for on-propulsion consumption .thruster(Thruster { // "NEXT-STEP" row in Table 2 isp_s: 4435.0, @@ -125,9 +124,9 @@ fn main() -> Result<(), Box> { .with(sc, almanac.clone()) .for_duration_with_traj(prop_time)?; - let fuel_usage = sc.fuel_mass_kg - final_state.fuel_mass_kg; + let prop_usage = sc.mass.prop_mass_kg - final_state.mass.prop_mass_kg; println!("{:x}", final_state.orbit); - println!("fuel usage: {:.3} kg", fuel_usage); + println!("prop usage: {:.3} kg", prop_usage); // Finally, export the results for analysis, including the penumbra percentage throughout the orbit raise. traj.to_parquet( diff --git a/examples/03_geo_analysis/stationkeeping.rs b/examples/03_geo_analysis/stationkeeping.rs index 8fdc730c..972c77cf 100644 --- a/examples/03_geo_analysis/stationkeeping.rs +++ b/examples/03_geo_analysis/stationkeeping.rs @@ -12,7 +12,7 @@ use anise::{ }; use hifitime::{Epoch, TimeUnits, Unit}; use nyx::{ - cosmic::{eclipse::EclipseLocator, GuidanceMode, MetaAlmanac, Orbit, SrpConfig}, + cosmic::{eclipse::EclipseLocator, GuidanceMode, Mass, MetaAlmanac, Orbit, SRPData}, dynamics::{ guidance::{Ruggiero, Thruster}, Harmonics, OrbitalDynamics, SolarPressure, SpacecraftDynamics, @@ -38,9 +38,8 @@ fn main() -> Result<(), Box> { let sc = Spacecraft::builder() .orbit(orbit) - .dry_mass_kg(1000.0) // 1000 kg of dry mass - .fuel_mass_kg(1000.0) // 1000 kg of fuel, totalling 2.0 tons - .srp(SrpConfig::from_area(3.0 * 6.0)) // Assuming 1 kW/m^2 or 18 kW, giving a margin of 4.35 kW for on-propulsion consumption + .mass(Mass::from_dry_and_prop_masses(1000.0, 1000.0)) // 1000 kg of dry mass and prop, totalling 2.0 tons + .srp(SRPData::from_area(3.0 * 6.0)) // Assuming 1 kW/m^2 or 18 kW, giving a margin of 4.35 kW for on-propulsion consumption .thruster(Thruster { // "NEXT-STEP" row in Table 2 isp_s: 4435.0, diff --git a/examples/04_lro_od/main.rs b/examples/04_lro_od/main.rs index 69e61b91..81dc2bff 100644 --- a/examples/04_lro_od/main.rs +++ b/examples/04_lro_od/main.rs @@ -12,7 +12,7 @@ use anise::{ }; use hifitime::{Epoch, TimeUnits, Unit}; use nyx::{ - cosmic::{Aberration, Frame, MetaAlmanac, SrpConfig}, + cosmic::{Aberration, Frame, Mass, MetaAlmanac, SRPData}, dynamics::{ guidance::LocalFrame, Harmonics, OrbitalDynamics, SolarPressure, SpacecraftDynamics, }, @@ -77,12 +77,11 @@ fn main() -> Result<(), Box> { // To build the trajectory we need to provide a spacecraft template. let sc_template = Spacecraft::builder() - .dry_mass_kg(1018.0) // Launch masses - .fuel_mass_kg(900.0) - .srp(SrpConfig { + .mass(Mass::from_dry_and_prop_masses(1018.0, 900.0)) // Launch masses + .srp(SRPData { // SRP configuration is arbitrary, but we will be estimating it anyway. area_m2: 3.9 * 2.7, - cr: 0.96, + coeff_reflectivity: 0.96, }) .orbit(Orbit::zero(MOON_J2000)) // Setting a zero orbit here because it's just a template .build(); @@ -125,7 +124,6 @@ fn main() -> Result<(), Box> { // The harmonics must be computed in the body fixed frame. // We're using the long term prediction of the Moon principal axes frame. let moon_pa_frame = MOON_PA_FRAME.with_orient(31008); - // let moon_pa_frame = IAU_MOON_FRAME; let sph_harmonics = Harmonics::from_stor( almanac.frame_from_uid(moon_pa_frame)?, HarmonicsMem::from_shadr(&jggrx_meta.uri, 80, 80, true)?, diff --git a/src/cosmic/bplane.rs b/src/cosmic/bplane.rs index 8dace89d..97b99bf9 100644 --- a/src/cosmic/bplane.rs +++ b/src/cosmic/bplane.rs @@ -27,15 +27,14 @@ use crate::time::{Duration, Epoch, Unit}; use crate::utils::between_pm_180; use hyperdual::linalg::norm; use hyperdual::{Float, OHyperdual}; -#[cfg(feature = "python")] -use pyo3::prelude::*; + use snafu::{ensure, ResultExt}; use std::convert::From; use std::fmt; /// Stores a B-Plane #[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "python", pyclass)] + pub struct BPlane { /// The $B_T$ component, in kilometers pub b_t: OrbitPartial, @@ -186,7 +185,6 @@ impl BPlane { } } -#[cfg_attr(feature = "python", pymethods)] impl BPlane { pub fn b_dot_t(&self) -> f64 { self.b_t.real() @@ -209,16 +207,6 @@ impl BPlane { pub fn mag(&self) -> f64 { (self.b_dot_t().powi(2) + self.b_dot_r().powi(2)).sqrt() } - - #[cfg(feature = "python")] - pub fn __repr__(&self) -> String { - format!("{self:?}") - } - - #[cfg(feature = "python")] - pub fn __str__(&self) -> String { - format!("{self}") - } } impl fmt::Display for BPlane { diff --git a/src/cosmic/mod.rs b/src/cosmic/mod.rs index 4352e9b7..10fc02c3 100644 --- a/src/cosmic/mod.rs +++ b/src/cosmic/mod.rs @@ -19,7 +19,7 @@ use anise::errors::{AlmanacError, PhysicsError}; pub use anise::prelude::*; -pub use crate::cosmic::{GuidanceMode, Spacecraft}; +pub use crate::cosmic::{DragData, GuidanceMode, Mass, SRPData, Spacecraft}; use crate::dynamics::DynamicsError; pub use crate::errors::NyxError; use crate::errors::StateError; diff --git a/src/cosmic/spacecraft.rs b/src/cosmic/spacecraft.rs index 9f427ebf..ae02cb16 100644 --- a/src/cosmic/spacecraft.rs +++ b/src/cosmic/spacecraft.rs @@ -20,6 +20,7 @@ use anise::astro::PhysicsResult; use anise::constants::frames::EARTH_J2000; pub use anise::prelude::Orbit; +pub use anise::structure::spacecraft::{DragData, Mass, SRPData}; use nalgebra::Vector3; use serde::{Deserialize, Serialize}; use snafu::ResultExt; @@ -35,14 +36,12 @@ use crate::md::StateParameter; use crate::time::Epoch; use crate::utils::{cartesian_to_spherical, spherical_to_cartesian}; -#[cfg(feature = "python")] -use pyo3::prelude::*; use std::default::Default; use std::fmt; use std::ops::Add; #[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)] -#[cfg_attr(feature = "python", pyclass)] + pub enum GuidanceMode { /// Guidance is turned off and Guidance Law may switch mode to Thrust for next call Coast, @@ -80,30 +79,23 @@ impl From for f64 { } } -/// A spacecraft state, composed of its orbit, its dry and fuel (wet) masses (in kg), its SRP configuration, its drag configuration, its thruster configuration, and its guidance mode. +/// 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. /// /// 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). #[derive(Clone, Copy, Debug, Serialize, Deserialize, TypedBuilder)] -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.cosmic"))] pub struct Spacecraft { - /// Initial orbit the vehicle is in + /// Initial orbit of the vehicle pub orbit: Orbit, - /// Dry mass, i.e. mass without fuel, in kg - #[builder(default)] - pub dry_mass_kg: f64, - /// Fuel mass (if fuel mass is negative, thrusting will fail, unless configured to break laws of physics) + /// Dry, propellant, and extra masses #[builder(default)] - pub fuel_mass_kg: f64, + pub mass: Mass, /// Solar Radiation Pressure configuration for this spacecraft #[builder(default)] #[serde(default)] - pub srp: SrpConfig, - + pub srp: SRPData, #[builder(default)] #[serde(default)] - pub drag: DragConfig, - + pub drag: DragData, #[builder(default, setter(strip_option))] pub thruster: Option, /// Any extra information or extension that is needed for specific guidance laws @@ -111,7 +103,7 @@ pub struct Spacecraft { #[serde(default)] pub mode: GuidanceMode, /// Optionally stores the state transition matrix from the start of the propagation until the current time (i.e. trajectory STM, not step-size STM) - /// STM is contains position and velocity, Cr, Cd, fuel mass + /// STM is contains position and velocity, Cr, Cd, prop mass #[builder(default, setter(strip_option))] #[serde(skip)] pub stm: Option, Const<9>>>, @@ -121,10 +113,9 @@ impl Default for Spacecraft { fn default() -> Self { Self { orbit: Orbit::zero(EARTH_J2000), - dry_mass_kg: 0.0, - fuel_mass_kg: 0.0, - srp: SrpConfig::default(), - drag: DragConfig::default(), + mass: Mass::default(), + srp: SRPData::default(), + drag: DragData::default(), thruster: None, mode: GuidanceMode::default(), stm: None, @@ -138,88 +129,27 @@ impl From for Spacecraft { } } -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.cosmic"))] -#[derive(Copy, Clone, Debug, Serialize, Deserialize, PartialEq)] -/// The Solar Radiation Pressure configuration for a spacecraft -pub struct SrpConfig { - /// solar radiation pressure area - pub area_m2: f64, - /// coefficient of reflectivity, must be between 0.0 (translucent) and 2.0 (all radiation absorbed and twice the force is transmitted back), defaults to 1.8 - pub cr: f64, -} - -impl SrpConfig { - /// Initialize the SRP from the c_r default and the provided drag area - pub fn from_area(area_m2: f64) -> Self { - Self { - area_m2, - ..Default::default() - } - } -} - -impl Default for SrpConfig { - fn default() -> Self { - Self { - area_m2: 0.0, - cr: 1.8, - } - } -} - -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.cosmic"))] -#[derive(Copy, Clone, Debug, Serialize, Deserialize, PartialEq)] -/// The drag configuration for a spacecraft -pub struct DragConfig { - /// drag area - pub area_m2: f64, - /// coefficient of drag; (spheres are between 2.0 and 2.1, use 2.2 in Earth's atmosphere (default)). - pub cd: f64, -} - -impl DragConfig { - /// Initialize the drag from the c_d default and the provided drag area - pub fn from_area(area_m2: f64) -> Self { - Self { - area_m2, - ..Default::default() - } - } -} - -impl Default for DragConfig { - fn default() -> Self { - Self { - area_m2: 0.0, - cd: 2.2, - } - } -} - impl Spacecraft { /// Initialize a spacecraft state from all of its parameters pub fn new( orbit: Orbit, dry_mass_kg: f64, - fuel_mass_kg: f64, + prop_mass_kg: f64, srp_area_m2: f64, drag_area_m2: f64, - cr: f64, - cd: f64, + coeff_reflectivity: f64, + coeff_drag: f64, ) -> Self { Self { orbit, - dry_mass_kg, - fuel_mass_kg, - srp: SrpConfig { + mass: Mass::from_dry_and_prop_masses(dry_mass_kg, prop_mass_kg), + srp: SRPData { area_m2: srp_area_m2, - cr, + coeff_reflectivity, }, - drag: DragConfig { + drag: DragData { area_m2: drag_area_m2, - cd, + coeff_drag, }, ..Default::default() } @@ -229,36 +159,35 @@ impl Spacecraft { pub fn from_thruster( orbit: Orbit, dry_mass_kg: f64, - fuel_mass_kg: f64, + prop_mass_kg: f64, thruster: Thruster, mode: GuidanceMode, ) -> Self { Self { orbit, - dry_mass_kg, - fuel_mass_kg, + mass: Mass::from_dry_and_prop_masses(dry_mass_kg, prop_mass_kg), thruster: Some(thruster), mode, ..Default::default() } } - /// Initialize a spacecraft state from the SRP default 1.8 for coefficient of reflectivity (fuel mass and drag parameters nullified!) + /// Initialize a spacecraft state from the SRP default 1.8 for coefficient of reflectivity (prop mass and drag parameters nullified!) pub fn from_srp_defaults(orbit: Orbit, dry_mass_kg: f64, srp_area_m2: f64) -> Self { Self { orbit, - dry_mass_kg, - srp: SrpConfig::from_area(srp_area_m2), + mass: Mass::from_dry_mass(dry_mass_kg), + srp: SRPData::from_area(srp_area_m2), ..Default::default() } } - /// Initialize a spacecraft state from the SRP default 1.8 for coefficient of drag (fuel mass and SRP parameters nullified!) + /// Initialize a spacecraft state from the SRP default 1.8 for coefficient of drag (prop mass and SRP parameters nullified!) pub fn from_drag_defaults(orbit: Orbit, dry_mass_kg: f64, drag_area_m2: f64) -> Self { Self { orbit, - dry_mass_kg, - drag: DragConfig::from_area(drag_area_m2), + mass: Mass::from_dry_mass(dry_mass_kg), + drag: DragData::from_area(drag_area_m2), ..Default::default() } } @@ -270,21 +199,21 @@ impl Spacecraft { /// Returns a copy of the state with a new dry mass pub fn with_dry_mass(mut self, dry_mass_kg: f64) -> Self { - self.dry_mass_kg = dry_mass_kg; + self.mass.dry_mass_kg = dry_mass_kg; self } - /// Returns a copy of the state with a new fuel mass - pub fn with_fuel_mass(mut self, fuel_mass_kg: f64) -> Self { - self.fuel_mass_kg = fuel_mass_kg; + /// Returns a copy of the state with a new prop mass + pub fn with_prop_mass(mut self, prop_mass_kg: f64) -> Self { + self.mass.prop_mass_kg = prop_mass_kg; self } /// Returns a copy of the state with a new SRP area and CR - pub fn with_srp(mut self, srp_area_m2: f64, cr: f64) -> Self { - self.srp = SrpConfig { + pub fn with_srp(mut self, srp_area_m2: f64, coeff_reflectivity: f64) -> Self { + self.srp = SRPData { area_m2: srp_area_m2, - cr, + coeff_reflectivity, }; self @@ -297,16 +226,16 @@ impl Spacecraft { } /// Returns a copy of the state with a new coefficient of reflectivity - pub fn with_cr(mut self, cr: f64) -> Self { - self.srp.cr = cr; + pub fn with_cr(mut self, coeff_reflectivity: f64) -> Self { + self.srp.coeff_reflectivity = coeff_reflectivity; self } /// Returns a copy of the state with a new drag area and CD - pub fn with_drag(mut self, drag_area_m2: f64, cd: f64) -> Self { - self.drag = DragConfig { + pub fn with_drag(mut self, drag_area_m2: f64, coeff_drag: f64) -> Self { + self.drag = DragData { area_m2: drag_area_m2, - cd, + coeff_drag, }; self } @@ -318,8 +247,8 @@ impl Spacecraft { } /// Returns a copy of the state with a new coefficient of drag - pub fn with_cd(mut self, cd: f64) -> Self { - self.drag.cd = cd; + pub fn with_cd(mut self, coeff_drag: f64) -> Self { + self.drag.coeff_drag = coeff_drag; self } @@ -329,13 +258,15 @@ impl Spacecraft { self } - /// 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 fuel + /// 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 pub fn rss(&self, other: &Self) -> PhysicsResult<(f64, f64, f64)> { let rss_p_km = self.orbit.rss_radius_km(&other.orbit)?; let rss_v_km_s = self.orbit.rss_velocity_km_s(&other.orbit)?; - let rss_fuel_kg = (self.fuel_mass_kg - other.fuel_mass_kg).powi(2).sqrt(); + let rss_prop_kg = (self.mass.prop_mass_kg - other.mass.prop_mass_kg) + .powi(2) + .sqrt(); - Ok((rss_p_km, rss_v_km_s, rss_fuel_kg)) + Ok((rss_p_km, rss_v_km_s, rss_prop_kg)) } /// Sets the STM of this state of identity, which also enables computation of the STM for spacecraft navigation @@ -351,7 +282,7 @@ impl Spacecraft { /// Returns the total mass in kilograms pub fn mass_kg(&self) -> f64 { - self.dry_mass_kg + self.fuel_mass_kg + self.mass.total_mass_kg() } /// Returns a copy of the state with the provided guidance mode @@ -373,8 +304,7 @@ impl PartialEq for Spacecraft { fn eq(&self, other: &Self) -> bool { let mass_tol = 1e-6; // milligram self.orbit.eq_within(&other.orbit, 1e-9, 1e-12) - && (self.dry_mass_kg - other.dry_mass_kg).abs() < mass_tol - && (self.fuel_mass_kg - other.fuel_mass_kg).abs() < mass_tol + && (self.mass - other.mass).abs().total_mass_kg() < mass_tol && self.srp == other.srp && self.drag == other.drag } @@ -388,7 +318,7 @@ impl fmt::Display for Spacecraft { write!( f, "total mass = {} kg @ {} {:?}", - format!("{:.*}", mass_prec, self.dry_mass_kg + self.fuel_mass_kg), + format!("{:.*}", mass_prec, self.mass.total_mass_kg()), format!("{:.*}", orbit_prec, self.orbit), self.mode, ) @@ -403,7 +333,7 @@ impl fmt::LowerExp for Spacecraft { write!( f, "total mass = {} kg @ {} {:?}", - format!("{:.*e}", mass_prec, self.dry_mass_kg + self.fuel_mass_kg), + format!("{:.*e}", mass_prec, self.mass.total_mass_kg()), format!("{:.*e}", orbit_prec, self.orbit), self.mode, ) @@ -418,7 +348,7 @@ impl fmt::LowerHex for Spacecraft { write!( f, "total mass = {} kg @ {} {:?}", - format!("{:.*}", mass_prec, self.dry_mass_kg + self.fuel_mass_kg), + format!("{:.*}", mass_prec, self.mass.total_mass_kg()), format!("{:.*x}", orbit_prec, self.orbit), self.mode, ) @@ -433,7 +363,7 @@ impl fmt::UpperHex for Spacecraft { write!( f, "total mass = {} kg @ {} {:?}", - format!("{:.*e}", mass_prec, self.dry_mass_kg + self.fuel_mass_kg), + format!("{:.*e}", mass_prec, self.mass.total_mass_kg()), format!("{:.*X}", orbit_prec, self.orbit), self.mode, ) @@ -466,9 +396,9 @@ impl State for Spacecraft { vector[i + 3] = *val; } // Set the spacecraft parameters - vector[6] = self.srp.cr; - vector[7] = self.drag.cd; - vector[8] = self.fuel_mass_kg; + vector[6] = self.srp.coeff_reflectivity; + vector[7] = self.drag.coeff_drag; + vector[8] = self.mass.prop_mass_kg; // Add the STM to the vector if let Some(stm) = self.stm { for (idx, stm_val) in stm.as_slice().iter().enumerate() { @@ -497,13 +427,13 @@ impl State for Spacecraft { self.orbit.epoch = epoch; self.orbit.radius_km = radius_km; self.orbit.velocity_km_s = vel_km_s; - self.srp.cr = sc_state[6].clamp(0.0, 2.0); - self.drag.cd = sc_state[7]; - self.fuel_mass_kg = sc_state[8]; + self.srp.coeff_reflectivity = sc_state[6].clamp(0.0, 2.0); + self.drag.coeff_drag = sc_state[7]; + self.mass.prop_mass_kg = sc_state[8]; } /// diag(STM) = [X,Y,Z,Vx,Vy,Vz,Cr,Cd,Fuel] - /// WARNING: Currently the STM assumes that the fuel mass is constant at ALL TIMES! + /// WARNING: Currently the STM assumes that the prop mass is constant at ALL TIMES! fn stm(&self) -> Result, DynamicsError> { match self.stm { Some(stm) => Ok(stm), @@ -525,10 +455,11 @@ impl State for Spacecraft { fn value(&self, param: StateParameter) -> Result { match param { - StateParameter::Cd => Ok(self.drag.cd), - StateParameter::Cr => Ok(self.srp.cr), - StateParameter::DryMass => Ok(self.dry_mass_kg), - StateParameter::FuelMass => Ok(self.fuel_mass_kg), + StateParameter::Cd => Ok(self.drag.coeff_drag), + StateParameter::Cr => Ok(self.srp.coeff_reflectivity), + StateParameter::DryMass => Ok(self.mass.dry_mass_kg), + StateParameter::PropMass => Ok(self.mass.prop_mass_kg), + StateParameter::TotalMass => Ok(self.mass.total_mass_kg()), StateParameter::Isp => match self.thruster { Some(thruster) => Ok(thruster.isp_s), None => Err(StateError::NoThrusterAvail), @@ -694,9 +625,10 @@ impl State for Spacecraft { fn set_value(&mut self, param: StateParameter, val: f64) -> Result<(), StateError> { match param { - StateParameter::Cd => self.drag.cd = val, - StateParameter::Cr => self.srp.cr = val, - StateParameter::FuelMass => self.fuel_mass_kg = val, + StateParameter::Cd => self.drag.coeff_drag = val, + StateParameter::Cr => self.srp.coeff_reflectivity = val, + StateParameter::PropMass => self.mass.prop_mass_kg = val, + StateParameter::DryMass => self.mass.dry_mass_kg = val, StateParameter::Isp => match self.thruster { Some(ref mut thruster) => thruster.isp_s = val, None => return Err(StateError::NoThrusterAvail), @@ -796,9 +728,9 @@ impl Add>> for Spacecraft { self.orbit.radius_km += radius_km; self.orbit.velocity_km_s += vel_km_s; - self.srp.cr = (self.srp.cr + other[6]).clamp(0.0, 2.0); - self.drag.cd += other[7]; - self.fuel_mass_kg += other[8]; + self.srp.coeff_reflectivity = (self.srp.coeff_reflectivity + other[6]).clamp(0.0, 2.0); + self.drag.coeff_drag += other[7]; + self.mass.prop_mass_kg += other[8]; self } @@ -850,14 +782,16 @@ orbit: orientation_id: 1 mu_km3_s2: null shape: null -dry_mass_kg: 500.0 -fuel_mass_kg: 159.0 +mass: + dry_mass_kg: 500.0 + prop_mass_kg: 159.0 + extra_mass_kg: 0.0 srp: area_m2: 2.0 - cr: 1.8 + coeff_reflectivity: 1.8 drag: area_m2: 2.0 - cd: 2.2 + coeff_drag: 2.2 "#; let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap(); @@ -880,14 +814,16 @@ orbit: orientation_id: 1 mu_km3_s2: null shape: null -dry_mass_kg: 500.0 -fuel_mass_kg: 159.0 +mass: + dry_mass_kg: 500.0 + prop_mass_kg: 159.0 + extra_mass_kg: 0.0 srp: area_m2: 2.0 - cr: 1.8 + coeff_reflectivity: 1.8 drag: area_m2: 2.0 - cd: 2.2 + coeff_drag: 2.2 thruster: thrust_N: 1e-5 isp_s: 300.5 @@ -918,8 +854,10 @@ orbit: orientation_id: 1 mu_km3_s2: null shape: null -dry_mass_kg: 500.0 -fuel_mass_kg: 159.0 +mass: + dry_mass_kg: 500.0 + prop_mass_kg: 159.0 + extra_mass_kg: 0.0 "#; let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap(); diff --git a/src/dynamics/deltavctrl.rs b/src/dynamics/deltavctrl.rs index 994048aa..5be1cbbb 100644 --- a/src/dynamics/deltavctrl.rs +++ b/src/dynamics/deltavctrl.rs @@ -19,7 +19,7 @@ use crate::cosmic::Orbit; use crate::linalg::Vector3; -pub use super::guidance::Mnvr; +pub use super::guidance::Maneuver; /// The `DeltaVctrl` trait handles control laws, optimizations, and other such methods for /// controlling the change in velocity of a point mass during a mission arc (`MissionArc`). @@ -37,13 +37,13 @@ where #[derive(Clone, Debug)] pub struct ImpulsiveBurns { /// Maneuvers should be provided in chronological order, first maneuver first in the list - pub mnvrs: Vec, + pub mnvrs: Vec, pub mnvr_no: usize, } impl ImpulsiveBurns { /// Builds a schedule from the vector of maneuvers, must be provided in chronological order. - pub fn from_mnvrs(mnvrs: Vec) -> Self { + pub fn from_mnvrs(mnvrs: Vec) -> Self { Self { mnvrs, mnvr_no: 0 } } } diff --git a/src/dynamics/drag.rs b/src/dynamics/drag.rs index b2d00202..24754b21 100644 --- a/src/dynamics/drag.rs +++ b/src/dynamics/drag.rs @@ -79,7 +79,13 @@ impl ForceModel for ConstantDrag { let velocity = osc.velocity_km_s; // Note the 1e3 factor to convert drag units from ((kg * km^2 * s^-2) / m^1) to (kg * km * s^-2) - Ok(-0.5 * 1e3 * self.rho * ctx.drag.cd * ctx.drag.area_m2 * velocity.norm() * velocity) + Ok(-0.5 + * 1e3 + * self.rho + * ctx.drag.coeff_drag + * ctx.drag.area_m2 + * velocity.norm() + * velocity) } fn dual_eom( @@ -170,7 +176,13 @@ impl ForceModel for Drag { AtmDensity::Constant(rho) => { let velocity = osc_drag_frame.velocity_km_s; // Note the 1e3 factor to convert drag units from ((kg * km^2 * s^-2) / m^1) to (kg * km * s^-2) - Ok(-0.5 * 1e3 * rho * ctx.drag.cd * ctx.drag.area_m2 * velocity.norm() * velocity) + Ok(-0.5 + * 1e3 + * rho + * ctx.drag.coeff_drag + * ctx.drag.area_m2 + * velocity.norm() + * velocity) } AtmDensity::Exponential { @@ -192,7 +204,6 @@ impl ForceModel for Drag { // TODO: Drag modeling will be improved in https://github.com/nyx-space/nyx/issues/317 // The frame will be double checked in this PR as well. - // let velocity_integr_frame = self.cosm.frame_chg(&osc, integration_frame).velocity(); let velocity_integr_frame = almanac .transform_to(osc_drag_frame, integration_frame, None) .context(DynamicsAlmanacSnafu { @@ -202,7 +213,13 @@ impl ForceModel for Drag { let velocity = velocity_integr_frame - osc_drag_frame.velocity_km_s; // Note the 1e3 factor to convert drag units from ((kg * km^2 * s^-2) / m^1) to (kg * km * s^-2) - Ok(-0.5 * 1e3 * rho * ctx.drag.cd * ctx.drag.area_m2 * velocity.norm() * velocity) + Ok(-0.5 + * 1e3 + * rho + * ctx.drag.coeff_drag + * ctx.drag.area_m2 + * velocity.norm() + * velocity) } AtmDensity::StdAtm { max_alt_m } => { @@ -230,7 +247,6 @@ impl ForceModel for Drag { 10.0_f64.powf(logdensity) }; - // let velocity_integr_frame = self.cosm.frame_chg(&osc, integration_frame).velocity(); let velocity_integr_frame = almanac .transform_to(osc_drag_frame, integration_frame, None) .context(DynamicsAlmanacSnafu { @@ -240,7 +256,13 @@ impl ForceModel for Drag { let velocity = velocity_integr_frame - osc_drag_frame.velocity_km_s; // Note the 1e3 factor to convert drag units from ((kg * km^2 * s^-2) / m^1) to (kg * km * s^-2) - Ok(-0.5 * 1e3 * rho * ctx.drag.cd * ctx.drag.area_m2 * velocity.norm() * velocity) + Ok(-0.5 + * 1e3 + * rho + * ctx.drag.coeff_drag + * ctx.drag.area_m2 + * velocity.norm() + * velocity) } } } diff --git a/src/dynamics/guidance/finiteburns.rs b/src/dynamics/guidance/finiteburns.rs index d2336f96..37a3bc4f 100644 --- a/src/dynamics/guidance/finiteburns.rs +++ b/src/dynamics/guidance/finiteburns.rs @@ -18,27 +18,28 @@ use anise::prelude::Almanac; */ use hifitime::Epoch; -use super::{GuidanceError, GuidanceLaw, Mnvr}; +use super::{GuidanceError, GuidanceLaw, Maneuver}; use crate::cosmic::{GuidanceMode, Spacecraft}; use crate::linalg::Vector3; use crate::State; use std::fmt; use std::sync::Arc; -/// A controller for a set of pre-determined maneuvers. + +/// A guidance law for a set of pre-determined maneuvers. #[derive(Clone, Debug)] pub struct FiniteBurns { /// Maneuvers should be provided in chronological order, first maneuver first in the list - pub mnvrs: Vec, + pub mnvrs: Vec, } impl FiniteBurns { /// Builds a schedule from the vector of maneuvers, must be provided in chronological order. - pub fn from_mnvrs(mnvrs: Vec) -> Arc { + pub fn from_mnvrs(mnvrs: Vec) -> Arc { Arc::new(Self { mnvrs }) } /// Find the maneuver with the closest start epoch that is less than or equal to the current epoch - fn maneuver_at(&self, epoch: Epoch) -> Option<&Mnvr> { + fn maneuver_at(&self, epoch: Epoch) -> Option<&Maneuver> { let index = self.mnvrs.binary_search_by_key(&epoch, |mnvr| mnvr.start); match index { Err(0) => None, // No maneuvers start before the current epoch @@ -62,7 +63,7 @@ impl GuidanceLaw for FiniteBurns { GuidanceMode::Thrust => { if let Some(next_mnvr) = self.maneuver_at(osc.epoch()) { if next_mnvr.start <= osc.epoch() { - ::direction(next_mnvr, osc) + ::direction(next_mnvr, osc) } else { Ok(Vector3::zeros()) } diff --git a/src/dynamics/guidance/mnvr.rs b/src/dynamics/guidance/mnvr.rs index 61336295..79a277dc 100644 --- a/src/dynamics/guidance/mnvr.rs +++ b/src/dynamics/guidance/mnvr.rs @@ -27,14 +27,15 @@ use crate::time::{Epoch, Unit}; use crate::State; use anise::prelude::Almanac; use hifitime::{Duration, TimeUnits}; +use serde::{Deserialize, Serialize}; use snafu::ResultExt; use std::fmt; use std::sync::Arc; /// Mnvr defined a single maneuver. Direction MUST be in the VNC frame (Velocity / Normal / Cross). /// It may be used with a maneuver scheduler. -#[derive(Copy, Clone, Debug)] -pub struct Mnvr { +#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)] +pub struct Maneuver { /// Start epoch of the maneuver pub start: Epoch, /// End epoch of the maneuver @@ -43,15 +44,13 @@ pub struct Mnvr { /// Thrust level, if 1.0 use all thruster available at full power /// TODO: Convert this to a common polynomial as well to optimize throttle, throttle rate (and accel?) pub thrust_prct: f64, - /// The interpolation polynomial for the in-plane angle - pub alpha_inplane_radians: CommonPolynomial, - /// The interpolation polynomial for the out-of-plane angle - pub delta_outofplane_radians: CommonPolynomial, + /// The representation of this maneuver. + pub representation: MnvrRepr, /// The frame in which the maneuvers are defined. pub frame: LocalFrame, } -impl fmt::Display for Mnvr { +impl fmt::Display for Maneuver { /// Prints the polynomial with the least significant coefficients first #[allow(clippy::identity_op)] fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { @@ -66,11 +65,7 @@ impl fmt::Display for Mnvr { self.end - self.start, self.end, )?; - write!( - f, - "\n\tin-plane angle α: {}\n\tout-of-plane angle β: {}", - self.alpha_inplane_radians, self.delta_outofplane_radians - )?; + write!(f, "\n{}", self.representation)?; write!( f, "\n\tinitial dir: [{:.6}, {:.6}, {:.6}]\n\tfinal dir : [{:.6}, {:.6}, {:.6}]", @@ -79,14 +74,41 @@ impl fmt::Display for Mnvr { } else { write!( f, - "Impulsive maneuver @ {}\n\tin-plane angle α: {}\n\tout-of-plane angle β: {}", - self.start, self.alpha_inplane_radians, self.delta_outofplane_radians + "Impulsive maneuver @ {}\n{}", + self.start, self.representation ) } } } -impl Mnvr { +/// Defines the available maneuver representations. +#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)] +pub enum MnvrRepr { + /// Represents the maneuver as a fixed vector in the local frame. + Vector(Vector3), + /// Represents the maneuver as a polynominal of azimuth (right ascension / in-plane) and elevation (declination / out of plane) + Angles { + azimuth: CommonPolynomial, + elevation: CommonPolynomial, + }, +} + +impl fmt::Display for MnvrRepr { + /// Prints the polynomial with the least significant coefficients first + #[allow(clippy::identity_op)] + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + match self { + MnvrRepr::Vector(vector) => write!(f, "{vector}"), + MnvrRepr::Angles { azimuth, elevation } => write!( + f, + "\tazimuth (in-plane) α: {}\n\televation (out-of-plane) β: {}", + azimuth, elevation + ), + } + } +} + +impl Maneuver { /// Creates an impulsive maneuver whose vector is the deltaV. /// TODO: This should use William's algorithm pub fn from_impulsive(dt: Epoch, vector: Vector3, frame: LocalFrame) -> Self { @@ -101,24 +123,26 @@ impl Mnvr { vector: Vector3, frame: LocalFrame, ) -> Self { - // Convert to angles - let (alpha, delta) = ra_dec_from_unit_vector(vector); Self { start, end, thrust_prct: thrust_lvl, - alpha_inplane_radians: CommonPolynomial::Constant(alpha), - delta_outofplane_radians: CommonPolynomial::Constant(delta), + representation: MnvrRepr::Vector(vector), frame, } } /// Return the thrust vector computed at the provided epoch pub fn vector(&self, epoch: Epoch) -> Vector3 { - let t = (epoch - self.start).to_seconds(); - let alpha = self.alpha_inplane_radians.eval(t); - let delta = self.delta_outofplane_radians.eval(t); - unit_vector_from_ra_dec(alpha, delta) + match self.representation { + MnvrRepr::Vector(vector) => vector, + MnvrRepr::Angles { azimuth, elevation } => { + let t = (epoch - self.start).to_seconds(); + let alpha = azimuth.eval(t); + let delta = elevation.eval(t); + unit_vector_from_ra_dec(alpha, delta) + } + } } /// Return the duration of this maneuver @@ -133,9 +157,14 @@ impl Mnvr { /// Returns the direction of the burn at the start of the burn, useful for setting new angles pub fn direction(&self) -> Vector3 { - let alpha = self.alpha_inplane_radians.coeff_in_order(0).unwrap(); - let delta = self.delta_outofplane_radians.coeff_in_order(0).unwrap(); - unit_vector_from_ra_dec(alpha, delta) + match self.representation { + MnvrRepr::Vector(vector) => vector / vector.norm(), + MnvrRepr::Angles { azimuth, elevation } => { + let alpha = azimuth.coeff_in_order(0).unwrap(); + let delta = elevation.coeff_in_order(0).unwrap(); + unit_vector_from_ra_dec(alpha, delta) + } + } } /// Set the time-invariant direction for this finite burn while keeping the other components as they are @@ -145,12 +174,15 @@ impl Mnvr { /// Returns the rate of direction of the burn at the start of the burn, useful for setting new angles pub fn rate(&self) -> Vector3 { - match self.alpha_inplane_radians.coeff_in_order(1) { - Ok(alpha) => { - let delta = self.delta_outofplane_radians.coeff_in_order(1).unwrap(); - unit_vector_from_ra_dec(alpha, delta) - } - Err(_) => Vector3::zeros(), + match self.representation { + MnvrRepr::Vector(_) => Vector3::zeros(), + MnvrRepr::Angles { azimuth, elevation } => match azimuth.coeff_in_order(1) { + Ok(alpha) => { + let delta = elevation.coeff_in_order(1).unwrap(); + unit_vector_from_ra_dec(alpha, delta) + } + Err(_) => Vector3::zeros(), + }, } } @@ -161,12 +193,15 @@ impl Mnvr { /// Returns the acceleration of the burn at the start of the burn, useful for setting new angles pub fn accel(&self) -> Vector3 { - match self.alpha_inplane_radians.coeff_in_order(2) { - Ok(alpha) => { - let delta = self.delta_outofplane_radians.coeff_in_order(2).unwrap(); - unit_vector_from_ra_dec(alpha, delta) - } - Err(_) => Vector3::zeros(), + match self.representation { + MnvrRepr::Vector(_) => Vector3::zeros(), + MnvrRepr::Angles { azimuth, elevation } => match azimuth.coeff_in_order(2) { + Ok(alpha) => { + let delta = elevation.coeff_in_order(2).unwrap(); + unit_vector_from_ra_dec(alpha, delta) + } + Err(_) => Vector3::zeros(), + }, } } @@ -182,55 +217,65 @@ impl Mnvr { rate: Vector3, accel: Vector3, ) -> Result<(), GuidanceError> { - let (alpha, delta) = ra_dec_from_unit_vector(dir); - if alpha.is_nan() || delta.is_nan() { - return Err(GuidanceError::InvalidDirection { - x: dir[0], - y: dir[1], - z: dir[2], - in_plane_deg: alpha.to_degrees(), - out_of_plane_deg: delta.to_degrees(), - }); - } - if rate.norm() < 2e-16 && accel.norm() < 2e-16 { - self.alpha_inplane_radians = CommonPolynomial::Constant(alpha); - self.delta_outofplane_radians = CommonPolynomial::Constant(delta); + if rate.norm() < f64::EPSILON && accel.norm() < f64::EPSILON { + // Set as a vector + self.representation = MnvrRepr::Vector(dir) } else { - let (alpha_dt, delta_dt) = ra_dec_from_unit_vector(rate); - if alpha_dt.is_nan() || delta_dt.is_nan() { - return Err(GuidanceError::InvalidRate { - x: rate[0], - y: rate[1], - z: rate[2], - in_plane_deg_s: alpha_dt.to_degrees(), - out_of_plane_deg_s: delta_dt.to_degrees(), + let (alpha, delta) = ra_dec_from_unit_vector(dir); + if alpha.is_nan() || delta.is_nan() { + return Err(GuidanceError::InvalidDirection { + x: dir[0], + y: dir[1], + z: dir[2], + in_plane_deg: alpha.to_degrees(), + out_of_plane_deg: delta.to_degrees(), }); } - if accel.norm() < 2e-16 { - self.alpha_inplane_radians = CommonPolynomial::Linear(alpha_dt, alpha); - self.delta_outofplane_radians = CommonPolynomial::Linear(delta_dt, delta); + if rate.norm() < f64::EPSILON && accel.norm() < f64::EPSILON { + self.representation = MnvrRepr::Angles { + azimuth: CommonPolynomial::Constant(alpha), + elevation: CommonPolynomial::Constant(delta), + }; } else { - let (alpha_ddt, delta_ddt) = ra_dec_from_unit_vector(accel); - if alpha_ddt.is_nan() || delta_ddt.is_nan() { - return Err(GuidanceError::InvalidAcceleration { - x: accel[0], - y: accel[1], - z: accel[2], - in_plane_deg_s2: alpha_ddt.to_degrees(), - out_of_plane_deg_s2: delta_ddt.to_degrees(), + let (alpha_dt, delta_dt) = ra_dec_from_unit_vector(rate); + if alpha_dt.is_nan() || delta_dt.is_nan() { + return Err(GuidanceError::InvalidRate { + x: rate[0], + y: rate[1], + z: rate[2], + in_plane_deg_s: alpha_dt.to_degrees(), + out_of_plane_deg_s: delta_dt.to_degrees(), }); } - self.alpha_inplane_radians = - CommonPolynomial::Quadratic(alpha_ddt, alpha_dt, alpha); - self.delta_outofplane_radians = - CommonPolynomial::Quadratic(delta_ddt, delta_dt, delta); + if accel.norm() < f64::EPSILON { + self.representation = MnvrRepr::Angles { + azimuth: CommonPolynomial::Linear(alpha_dt, alpha), + elevation: CommonPolynomial::Linear(delta_dt, delta), + }; + } else { + let (alpha_ddt, delta_ddt) = ra_dec_from_unit_vector(accel); + if alpha_ddt.is_nan() || delta_ddt.is_nan() { + return Err(GuidanceError::InvalidAcceleration { + x: accel[0], + y: accel[1], + z: accel[2], + in_plane_deg_s2: alpha_ddt.to_degrees(), + out_of_plane_deg_s2: delta_ddt.to_degrees(), + }); + } + + self.representation = MnvrRepr::Angles { + azimuth: CommonPolynomial::Quadratic(alpha_ddt, alpha_dt, alpha), + elevation: CommonPolynomial::Quadratic(delta_ddt, delta_dt, delta), + }; + } } } Ok(()) } } -impl GuidanceLaw for Mnvr { +impl GuidanceLaw for Maneuver { fn direction(&self, osc: &Spacecraft) -> Result, GuidanceError> { match osc.mode() { GuidanceMode::Thrust => match self.frame { @@ -265,3 +310,25 @@ impl GuidanceLaw for Mnvr { sc.mut_mode(next_mode); } } + +#[cfg(test)] +mod ut_mnvr { + use hifitime::Epoch; + use nalgebra::Vector3; + + use crate::dynamics::guidance::LocalFrame; + + use super::Maneuver; + + #[test] + fn serde_mnvr() { + let epoch = Epoch::from_gregorian_utc_at_midnight(2012, 2, 29); + let mnvr = Maneuver::from_impulsive(epoch, Vector3::new(1.0, 1.0, 0.0), LocalFrame::RCN); + + let mnvr_yml = serde_yml::to_string(&mnvr).unwrap(); + println!("{mnvr_yml}"); + + let mnvr2 = serde_yml::from_str(&mnvr_yml).unwrap(); + assert_eq!(mnvr, mnvr2); + } +} diff --git a/src/dynamics/guidance/mod.rs b/src/dynamics/guidance/mod.rs index b28ab856..6daa70ff 100644 --- a/src/dynamics/guidance/mod.rs +++ b/src/dynamics/guidance/mod.rs @@ -29,7 +29,7 @@ mod finiteburns; pub use finiteburns::FiniteBurns; mod mnvr; -pub use mnvr::Mnvr; +pub use mnvr::{Maneuver, MnvrRepr}; mod ruggiero; pub use ruggiero::{Objective, Ruggiero, StateParameter}; @@ -38,11 +38,7 @@ use snafu::Snafu; use std::fmt; use std::sync::Arc; -#[cfg(feature = "python")] -use pyo3::prelude::*; - /// Defines a thruster with a maximum isp and a maximum thrust. -#[cfg_attr(feature = "python", pyclass)] #[allow(non_snake_case)] #[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)] pub struct Thruster { @@ -52,20 +48,11 @@ pub struct Thruster { pub isp_s: f64, } -#[cfg_attr(feature = "python", pymethods)] impl Thruster { /// Returns the exhaust velocity v_e in meters per second pub fn exhaust_velocity_m_s(&self) -> f64 { self.isp_s * STD_GRAVITY } - - /// Creates a new Thruster given its thrust in Newton and its Isp in seconds - #[allow(non_snake_case)] - #[cfg(feature = "python")] - #[new] - fn py_new(thrust_N: f64, isp_s: f64) -> Self { - Self { thrust_N, isp_s } - } } /// The `GuidanceLaw` trait handles guidance laws, optimizations, and other such methods for @@ -164,7 +151,7 @@ pub enum GuidanceError { } /// Local frame options, used notably for guidance laws. -#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[derive(Copy, Clone, Debug, PartialEq, Eq, Serialize, Deserialize)] pub enum LocalFrame { Inertial, RIC, @@ -195,8 +182,8 @@ fn ra_dec_from_vec() { loop { let unit_v = unit_vector_from_ra_dec(alpha, delta); let (alpha2, delta2) = ra_dec_from_unit_vector(unit_v); - assert!((alpha - alpha2).abs() < 2e-16); - assert!((delta - delta2).abs() < 2e-16); + assert!((alpha - alpha2).abs() < f64::EPSILON); + assert!((delta - delta2).abs() < f64::EPSILON); alpha += TAU * 0.1; // Increment right ascension by one tenth of a circle if alpha > PI { alpha = 0.0; diff --git a/src/dynamics/orbital.rs b/src/dynamics/orbital.rs index 1358f7b0..3ff1815e 100644 --- a/src/dynamics/orbital.rs +++ b/src/dynamics/orbital.rs @@ -152,7 +152,6 @@ impl OrbitalDynamics { // Apply the acceleration models for model in &self.accel_models { - // let (model_acc, model_grad) = model.dual_eom(&radius, osc)?; let (model_acc, model_grad) = model.dual_eom(osc, almanac.clone())?; for i in 0..3 { dx[i + 3] += model_acc[i]; diff --git a/src/dynamics/solarpressure.rs b/src/dynamics/solarpressure.rs index 05139654..f9655175 100644 --- a/src/dynamics/solarpressure.rs +++ b/src/dynamics/solarpressure.rs @@ -145,7 +145,7 @@ impl ForceModel for SolarPressure { let flux_pressure = (k * self.phi / SPEED_OF_LIGHT_M_S) * (1.0 / r_sun_au).powi(2); // Note the 1e-3 is to convert the SRP from m/s^2 to km/s^2 - Ok(1e-3 * ctx.srp.cr * ctx.srp.area_m2 * flux_pressure * r_sun_unit) + Ok(1e-3 * ctx.srp.coeff_reflectivity * ctx.srp.area_m2 * flux_pressure * r_sun_unit) } fn dual_eom( @@ -187,8 +187,9 @@ impl ForceModel for SolarPressure { * inv_r_sun_au_p2; // Note the 1e-3 is to convert the SRP from m/s^2 to km/s^2 - let dual_force_scalar = - OHyperdual::>::from_real(1e-3 * ctx.srp.cr * ctx.srp.area_m2); + let dual_force_scalar = OHyperdual::>::from_real( + 1e-3 * ctx.srp.coeff_reflectivity * ctx.srp.area_m2, + ); let mut dual_force: Vector3>> = Vector3::zeros(); dual_force[0] = dual_force_scalar * flux_pressure * r_sun_unit[0]; dual_force[1] = dual_force_scalar * flux_pressure * r_sun_unit[1]; @@ -206,7 +207,7 @@ impl ForceModel for SolarPressure { } // Compute the partial wrt to Cr. - let wrt_cr = self.eom(ctx, almanac)? / ctx.srp.cr; + let wrt_cr = self.eom(ctx, almanac)? / ctx.srp.coeff_reflectivity; for j in 0..3 { grad[(3, j)] = wrt_cr[j]; } diff --git a/src/dynamics/spacecraft.rs b/src/dynamics/spacecraft.rs index 14897452..0189bad2 100644 --- a/src/dynamics/spacecraft.rs +++ b/src/dynamics/spacecraft.rs @@ -33,29 +33,13 @@ use std::fmt::{self, Write}; use std::sync::Arc; use crate::cosmic::AstroError; -#[cfg(feature = "python")] -use crate::io::ConfigRepr; -#[cfg(feature = "python")] -use crate::python::PythonError; -#[cfg(feature = "python")] -use pyo3::class::basic::CompareOp; -#[cfg(feature = "python")] -use pyo3::prelude::*; -#[cfg(feature = "python")] -use pyo3::types::PyType; -#[cfg(feature = "python")] -use pythonize::depythonize; -#[cfg(feature = "python")] -use std::collections::BTreeMap; const NORM_ERR: f64 = 1e-4; -/// A generic spacecraft dynamics with associated force models, guidance law, and flag specifying whether to decrement the fuel mass or not. -/// Note: when developing new guidance laws, it is recommended to _not_ enable fuel decrement until the guidance law seems to work without proper physics. -/// Note: if the spacecraft runs out of fuel, the propagation segment will return an error. +/// A generic spacecraft dynamics with associated force models, guidance law, and flag specifying whether to decrement the prop mass or not. +/// Note: when developing new guidance laws, it is recommended to _not_ enable prop decrement until the guidance law seems to work without proper physics. +/// Note: if the spacecraft runs out of prop, the propagation segment will return an error. #[derive(Clone)] -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.mission_design"))] pub struct SpacecraftDynamics { pub orbital_dyn: OrbitalDynamics, // TODO: https://github.com/nyx-space/nyx/issues/214 @@ -77,7 +61,7 @@ impl SpacecraftDynamics { } /// Initialize a Spacecraft with a set of orbital dynamics and a propulsion subsystem. - /// Will _not_ decrement the fuel mass as propellant is consumed. + /// Will _not_ decrement the prop mass as propellant is consumed. pub fn from_guidance_law_no_decr( orbital_dyn: OrbitalDynamics, guid_law: Arc, @@ -139,75 +123,6 @@ impl SpacecraftDynamics { } } -#[cfg_attr(feature = "python", pymethods)] -impl SpacecraftDynamics { - #[cfg(feature = "python")] - #[classmethod] - fn load(_cls: &PyType, path: &str) -> Result { - let serde = DynamicsSerde::load(path)?; - - let cosm = Cosm::de438(); - - Self::from_config(serde, cosm) - } - - #[cfg(feature = "python")] - #[classmethod] - fn load_many(_cls: &PyType, path: &str) -> Result, ConfigError> { - let orbits = DynamicsSerde::load_many(path)?; - - let cosm = Cosm::de438(); - - let mut selves = Vec::with_capacity(orbits.len()); - - for serde in orbits { - selves.push(Self::from_config(serde, cosm.clone())?); - } - - Ok(selves) - } - - #[cfg(feature = "python")] - #[classmethod] - fn load_named(_cls: &PyType, path: &str) -> Result, ConfigError> { - let orbits = DynamicsSerde::load_named(path)?; - - let cosm = Cosm::de438(); - - let mut selves = BTreeMap::new(); - - for (k, v) in orbits { - selves.insert(k, Self::from_config(v, cosm.clone())?); - } - - Ok(selves) - } - - #[cfg(feature = "python")] - fn __repr__(&self) -> String { - format!("{self}") - } - - #[cfg(feature = "python")] - fn __richcmp__(&self, other: &Self, op: CompareOp) -> Result { - match op { - CompareOp::Eq => Ok(self.__repr__() == other.__repr__()), - CompareOp::Ne => Ok(self.__repr__() != other.__repr__()), - _ => Err(PythonError::OperationError { op }), - } - } - - #[cfg(feature = "python")] - #[classmethod] - /// Loads the SpacecraftDynamics from its YAML representation - fn loads(_cls: &PyType, state: &PyAny) -> Result { - ::from_config( - depythonize(state).map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() })?, - Cosm::de438(), - ) - } -} - impl fmt::Display for SpacecraftDynamics { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { let force_models: String = if self.force_models.is_empty() { @@ -239,8 +154,8 @@ impl Dynamics for SpacecraftDynamics { next_state: Self::StateType, almanac: Arc, ) -> Result { - if next_state.fuel_mass_kg < 0.0 { - error!("negative fuel mass at {}", next_state.epoch()); + if next_state.mass.prop_mass_kg < 0.0 { + error!("negative prop mass at {}", next_state.epoch()); return Err(DynamicsError::FuelExhausted { sc: Box::new(next_state), }); @@ -309,7 +224,7 @@ impl Dynamics for SpacecraftDynamics { // Now include the control as needed. if let Some(guid_law) = &self.guid_law { - let (thrust_force, fuel_rate) = { + let (thrust_force, prop_rate) = { if osc_sc.thruster.is_none() { return Err(DynamicsError::DynamicsGuidance { source: GuidanceError::NoThrustersDefined, @@ -345,9 +260,9 @@ impl Dynamics for SpacecraftDynamics { ( thrust_inertial * total_thrust, if self.decrement_mass { - let fuel_usage = thrust_throttle_lvl * thruster.thrust_N + let prop_usage = thrust_throttle_lvl * thruster.thrust_N / (thruster.isp_s * STD_GRAVITY); - -fuel_usage + -prop_usage } else { 0.0 }, @@ -367,7 +282,7 @@ impl Dynamics for SpacecraftDynamics { for i in 0..3 { d_x[i + 3] += thrust_force[i] / osc_sc.mass_kg(); } - d_x[8] += fuel_rate; + d_x[8] += prop_rate; } Ok(d_x) } diff --git a/src/io/cosmo.rs b/src/io/cosmo.rs deleted file mode 100644 index c43a03f0..00000000 --- a/src/io/cosmo.rs +++ /dev/null @@ -1,43 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use crate::cosmic::Orbit; -use csv::{QuoteStyle, Writer, WriterBuilder}; -use std::fs::File; - -/// Exports to the XYZV data type used in Cosmographia -pub struct Cosmographia { - wtr: Writer, -} - -impl Cosmographia { - pub fn from_path(path: String) -> Cosmographia { - Cosmographia { - wtr: WriterBuilder::new() - .delimiter(b' ') - .quote_style(QuoteStyle::Never) - .has_headers(false) - .from_path(path) - .expect("could not create file"), - } - } - - pub fn append(&mut self, s: Orbit) { - self.wtr.serialize(s).expect("could not write to XYZV file"); - } -} diff --git a/src/io/estimate.rs b/src/io/estimate.rs deleted file mode 100644 index 5dfcf91d..00000000 --- a/src/io/estimate.rs +++ /dev/null @@ -1,33 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use anise::astro::orbit::Orbit; -use serde::{Deserialize, Serialize}; - -use super::{matrices::Matrix6Serde, ConfigRepr}; - -/// Enables serializing and deserializing of an orbit estimate. -#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)] -pub struct OrbitEstimateSerde { - /// Expected nominal orbit - pub nominal: Orbit, - /// Covariance _must_ be specified as in Cartesian format - pub covar: Matrix6Serde, -} - -impl ConfigRepr for OrbitEstimateSerde {} diff --git a/src/io/matrices.rs b/src/io/matrices.rs deleted file mode 100644 index 619e2543..00000000 --- a/src/io/matrices.rs +++ /dev/null @@ -1,149 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use either::Either; -use nalgebra::{Matrix2, Matrix6, Vector2, Vector6}; -use serde::{Deserialize, Serialize}; - -// TODO: Use a macro to generate all of these implementations - -#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)] -#[serde(transparent)] -pub struct Matrix2Serde { - #[serde(with = "either::serde_untagged")] - inner: Either, -} - -impl Matrix2Serde { - pub fn to_matrix(&self) -> Matrix2 { - match self.inner { - Either::Left(diag) => Matrix2::from_diagonal(&Vector2::from_iterator(diag.0)), - Either::Right(mat2) => { - let mut flat: [f64; 4] = [0.0; 4]; - for (i, row) in mat2.0.iter().enumerate() { - for (j, val) in row.iter().enumerate() { - flat[4 * i + j] = *val; - } - } - Matrix2::from_row_slice(&flat) - } - } - } -} - -#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)] -pub struct Diag2([f64; 2]); - -#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)] -pub struct Mat2([[f64; 2]; 2]); - -#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)] -#[serde(transparent)] -pub struct Matrix6Serde { - #[serde(with = "either::serde_untagged")] - inner: Either, -} - -impl Matrix6Serde { - pub fn to_matrix(&self) -> Matrix6 { - match self.inner { - Either::Left(diag) => Matrix6::from_diagonal(&Vector6::from_iterator(diag.0)), - Either::Right(mat6) => { - let mut flat: [f64; 36] = [0.0; 36]; - for (i, row) in mat6.0.iter().enumerate() { - for (j, val) in row.iter().enumerate() { - flat[6 * i + j] = *val; - } - } - Matrix6::from_row_slice(&flat) - } - } - } -} - -#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)] -pub struct Diag6([f64; 6]); - -#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)] -pub struct Mat6([[f64; 6]; 6]); - -#[test] -fn test_serde2() { - use serde_yml; - - let m_diag = Matrix2Serde { - inner: Either::Left(Diag2([1.0, 2.0])), - }; - - println!("Diag -- \n{}", serde_yml::to_string(&m_diag).unwrap()); - // Load from one line list - let diag_s = "[1.0, 2.0]"; - let diag_loaded: Matrix2Serde = serde_yml::from_str(diag_s).unwrap(); - assert_eq!(diag_loaded, m_diag); - - let m_full = Matrix2Serde { - inner: Either::Right(Mat2([[1.0, 2.0]; 2])), - }; - - // Serialization will print this as an exhaustive list of lists. - println!("Full -- \n{}", serde_yml::to_string(&m_full).unwrap()); - // Load from list - let full_mat = r#" -- [1.0, 2.0] # Row 1 -- [1.0, 2.0] # Row 2 - "#; - - let full_loaded: Matrix2Serde = serde_yml::from_str(full_mat).unwrap(); - - assert_eq!(full_loaded, m_full); -} - -#[test] -fn test_serde6() { - use serde_yml; - - let m_diag = Matrix6Serde { - inner: Either::Left(Diag6([1.0, 2.0, 3.0, 4.0, 5.0, 6.0])), - }; - - println!("Diag -- \n{}", serde_yml::to_string(&m_diag).unwrap()); - // Load from one line list - let diag_s = "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0]"; - let diag_loaded: Matrix6Serde = serde_yml::from_str(diag_s).unwrap(); - assert_eq!(diag_loaded, m_diag); - - let m_full = Matrix6Serde { - inner: Either::Right(Mat6([[1.0, 2.0, 3.0, 4.0, 5.0, 6.0]; 6])), - }; - - // Serialization will print this as an exhaustive list of lists. - println!("Full -- \n{}", serde_yml::to_string(&m_full).unwrap()); - // Load from list - let full_mat = r#" -- [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # Row 1 -- [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # Row 2 -- [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # Row 3 -- [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # Row 4 -- [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # Row 5 -- [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # Row 6 - "#; - - let full_loaded: Matrix6Serde = serde_yml::from_str(full_mat).unwrap(); - - assert_eq!(full_loaded, m_full); -} diff --git a/src/io/mod.rs b/src/io/mod.rs index 4e241227..5678809c 100644 --- a/src/io/mod.rs +++ b/src/io/mod.rs @@ -40,23 +40,13 @@ use std::path::{Path, PathBuf}; use std::str::FromStr; use typed_builder::TypedBuilder; -/// Handles writing to an XYZV file -pub mod cosmo; -pub mod estimate; /// Handles loading of gravity models using files of NASA PDS and GMAT COF. Several gunzipped files are provided with nyx. pub mod gravity; -pub mod matrices; -// pub mod tracking_data; -pub mod trajectory_data; use std::io; -#[cfg(feature = "python")] -use pyo3::prelude::*; - /// Configuration for exporting a trajectory to parquet. #[derive(Clone, Default, Serialize, Deserialize, TypedBuilder)] -#[cfg_attr(feature = "python", pyclass)] #[builder(doc)] pub struct ExportCfg { /// Fields to export, if unset, defaults to all possible fields. @@ -131,31 +121,6 @@ impl ExportCfg { } } -#[cfg(feature = "python")] -#[pymethods] -impl ExportCfg { - #[new] - #[pyo3( - text_signature = "(timestamp=None, fields=None, start_epoch=None, step=None, end_epoch=None, metadata=None)" - )] - fn py_new( - timestamp: Option, - fields: Option>, - start_epoch: Option, - end_epoch: Option, - metadata: Option>, - ) -> Self { - Self { - timestamp: timestamp.unwrap_or(false), - fields, - start_epoch, - end_epoch, - metadata, - ..Default::default() - } - } -} - #[derive(Debug, Snafu)] #[snafu(visibility(pub(crate)))] pub enum ConfigError { @@ -204,7 +169,7 @@ pub enum InputOutputError { ParseDhall { data: String, err: String }, #[snafu(display("error serializing {what} to Dhall: {err}"))] SerializeDhall { what: String, err: String }, - #[snafu(display("empty dataset error when (de)serializing for {action}"))] + #[snafu(display("empty dataset error when (de)serializing {action}"))] EmptyDataset { action: &'static str }, } diff --git a/src/io/tracking_data.rs b/src/io/tracking_data.rs deleted file mode 100644 index 227796a3..00000000 --- a/src/io/tracking_data.rs +++ /dev/null @@ -1,319 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use crate::{ - io::{MissingDataSnafu, ParquetSnafu}, - linalg::{allocator::Allocator, DefaultAllocator, OVector}, - od::{msr::TrackingDataArc, Measurement}, -}; - -#[cfg(feature = "python")] -use crate::NyxError; - -use arrow::{ - array::{Float64Array, StringArray}, - record_batch::RecordBatchReader, -}; -use hifitime::Epoch; -use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder; -use snafu::prelude::*; -use std::fs::File; -use std::{collections::HashMap, error::Error, fmt::Display, path::Path}; - -#[cfg(feature = "python")] -use pyo3::prelude::*; - -use super::{InputOutputError, StdIOSnafu}; - -/// A dynamic tracking arc allows loading a set of measurements from a parquet file and converting them -/// to the concrete measurement type when desired. -#[cfg_attr(feature = "python", pyclass)] -pub struct DynamicTrackingArc { - pub device_cfg: String, - pub path: String, - metadata: HashMap, -} - -impl DynamicTrackingArc { - pub fn from_parquet>(path: P) -> Result> { - let file = File::open(&path)?; - - let builder = ParquetRecordBatchReaderBuilder::try_new(file).unwrap(); - - let mut metadata = HashMap::new(); - let mut device_cfg = String::new(); - // Build the custom metadata - if let Some(file_metadata) = builder.metadata().file_metadata().key_value_metadata() { - for key_value in file_metadata { - if !key_value.key.starts_with("ARROW:") { - metadata.insert( - key_value.key.clone(), - key_value.value.clone().unwrap_or("[unset]".to_string()), - ); - if key_value.key == "devices" { - device_cfg = key_value.value.clone().unwrap_or("[unset]".to_string()); - } - } - } - } - - let me = Self { - path: path.as_ref().to_string_lossy().to_string(), - metadata, - device_cfg, - }; - - for item in me.repr() { - info!("{item}"); - } - - Ok(me) - } - - /// Reads through the loaded parquet file and attempts to convert to the provided tracking arc. - pub fn to_tracking_arc(&self) -> Result, InputOutputError> - where - Msr: Measurement, - DefaultAllocator: Allocator, - { - // Read the file since we closed it earlier - let file = File::open(&self.path).context(StdIOSnafu { - action: "opening file for tracking arc", - })?; - let builder = ParquetRecordBatchReaderBuilder::try_new(file).unwrap(); - - let reader = builder.build().context(ParquetSnafu { - action: "reading tracking arc", - })?; - - // Check the schema - let mut has_epoch = false; - let mut has_tracking_dev = false; - let mut range_avail = false; - let mut rate_avail = false; - for field in &reader.schema().fields { - match field.name().as_str() { - "Epoch (UTC)" => has_epoch = true, - "Tracking device" => has_tracking_dev = true, - "Range (km)" => range_avail = true, - "Doppler (km/s)" => rate_avail = true, - _ => {} - } - } - - ensure!( - has_epoch, - MissingDataSnafu { - which: "Epoch (UTC)" - } - ); - - ensure!( - has_tracking_dev, - MissingDataSnafu { - which: "Tracking device" - } - ); - - ensure!( - range_avail || rate_avail, - MissingDataSnafu { - which: "`Range (km)` or `Doppler (km/s)`" - } - ); - - let expected_type = std::any::type_name::().split("::").last().unwrap(); - - // Only check that the file contains the data we need - match expected_type { - "RangeDoppler" => { - if !range_avail || !rate_avail { - return Err(InputOutputError::MissingData { - which: "`Range (km)` and `Doppler (km/s)`".to_string(), - }); - } - } - "RangeMsr" => { - if !range_avail { - return Err(InputOutputError::MissingData { - which: "`Range (km)`".to_string(), - }); - } - } - "RangeRate" => { - return Err(InputOutputError::MissingData { - which: "`Doppler (km/s)`".to_string(), - }); - } - _ => { - return Err(InputOutputError::UnsupportedData { - which: expected_type.to_string(), - }); - } - } - - // At this stage, we know that the measurement is valid and the conversion is supported. - let mut arc = TrackingArc { - device_cfg: self.device_cfg.clone(), - measurements: Vec::new(), - }; - - // Now convert each batch on the fly - for maybe_batch in reader { - let batch = maybe_batch.unwrap(); - - let tracking_device = batch - .column_by_name("Tracking device") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(); - - let epochs = batch - .column_by_name("Epoch (UTC)") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(); - - // Now read the data depending on what we're deserializing as - match expected_type { - "RangeDoppler" => { - let range_data = batch - .column_by_name("Range (km)") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(); - - let rate_data = batch - .column_by_name("Doppler (km/s)") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(); - - // Set the measurements in the tracking arc - for i in 0..batch.num_rows() { - arc.measurements.push(( - tracking_device.value(i).to_string(), - Msr::from_observation( - Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { - InputOutputError::Inconsistency { - msg: format!("{e} when parsing epoch"), - } - })?, - OVector::::from_iterator([ - range_data.value(i), - rate_data.value(i), - ]), - ), - )); - } - } - "RangeMsr" => { - let range_data = batch - .column_by_name("Range (km)") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(); - - // Set the measurements in the tracking arc - for i in 0..batch.num_rows() { - arc.measurements.push(( - tracking_device.value(i).to_string(), - Msr::from_observation( - Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { - InputOutputError::Inconsistency { - msg: format!("{e} when parsing epoch"), - } - })?, - OVector::::from_iterator([ - range_data.value(i) - ]), - ), - )); - } - } - "RangeRate" => { - let rate_data = batch - .column_by_name("Doppler (km/s)") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(); - - // Set the measurements in the tracking arc - for i in 0..batch.num_rows() { - arc.measurements.push(( - tracking_device.value(i).to_string(), - Msr::from_observation( - Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { - InputOutputError::Inconsistency { - msg: format!("{e} when parsing epoch"), - } - })?, - OVector::::from_iterator([ - rate_data.value(i) - ]), - ), - )); - } - } - _ => unreachable!("should have errored earlier"), - } - } - - Ok(arc) - } - - fn repr(&self) -> Vec { - let mut r = Vec::new(); - r.push(format!("File: {}", self.path)); - for (k, v) in &self.metadata { - if k != "devices" { - r.push(format!("{k}: {v}")); - } - } - r - } -} - -impl Display for DynamicTrackingArc { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - for item in self.repr() { - writeln!(f, "{item}")?; - } - Ok(()) - } -} - -#[cfg(feature = "python")] -#[pymethods] -impl DynamicTrackingArc { - /// Initializes a new dynamic tracking arc from the provided parquet file - #[new] - fn new(path: String) -> Result { - Self::from_parquet(path).map_err(|e| NyxError::CustomError { msg: e.to_string() }) - } - - fn __repr__(&self) -> String { - format!("{self}") - } -} diff --git a/src/io/trajectory_data.rs b/src/io/trajectory_data.rs deleted file mode 100644 index a6350348..00000000 --- a/src/io/trajectory_data.rs +++ /dev/null @@ -1,378 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use anise::frames::Frame; -use arrow::array::StringArray; -use arrow::{array::Float64Array, record_batch::RecordBatchReader}; -use hifitime::Epoch; -use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder; -use snafu::prelude::*; -use std::fs::File; -use std::{collections::HashMap, fmt::Display, path::Path}; - -use crate::{ - io::MissingDataSnafu, - linalg::{allocator::Allocator, DefaultAllocator}, - md::{prelude::Traj, trajectory::Interpolatable, StateParameter}, -}; - -#[cfg(feature = "python")] -use crate::python::mission_design::{OrbitTraj as OrbitTrajPy, SpacecraftTraj as ScTrajPy}; -#[cfg(feature = "python")] -use crate::python::PythonError; -#[cfg(feature = "python")] -use crate::NyxError; -#[cfg(feature = "python")] -use crate::Spacecraft; -#[cfg(feature = "python")] -use log::warn; -#[cfg(feature = "python")] -use pyo3::class::basic::CompareOp; -#[cfg(feature = "python")] -use pyo3::prelude::*; - -use super::{InputOutputError, ParquetSnafu, StdIOSnafu}; - -/// A dynamic trajectory allows loading a trajectory Parquet file and converting it -/// to the concrete trajectory state type when desired. -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.mission_design"))] -#[derive(Clone, PartialEq)] -pub struct TrajectoryLoader { - pub path: String, - metadata: HashMap, -} - -impl TrajectoryLoader { - pub fn from_parquet>(path: P) -> Result { - let file = File::open(&path).context(StdIOSnafu { - action: "opening trajectory file", - })?; - - let builder = ParquetRecordBatchReaderBuilder::try_new(file).unwrap(); - - let mut metadata = HashMap::new(); - // Build the custom metadata - if let Some(file_metadata) = builder.metadata().file_metadata().key_value_metadata() { - for key_value in file_metadata { - if !key_value.key.starts_with("ARROW:") { - metadata.insert( - key_value.key.clone(), - key_value.value.clone().unwrap_or("[unset]".to_string()), - ); - } - } - } - - let me = Self { - path: path.as_ref().to_string_lossy().to_string(), - metadata, - }; - - for item in me.repr() { - info!("{item}"); - } - - Ok(me) - } - - /// Reads through the loaded parquet file and attempts to convert to the provided concrete state. - /// - /// # Design limitations - /// For Python compatibility, the file is actually re-read here, although it was read and closed during initialization. - /// This is required because the parquet file reader is not clonable. - pub fn to_traj(&self) -> Result, InputOutputError> - where - S: Interpolatable, - DefaultAllocator: - Allocator + Allocator + Allocator, - { - // Check the schema - let mut has_epoch = false; // Required - let mut frame = None; - - let mut found_fields = vec![ - (StateParameter::X, false), - (StateParameter::Y, false), - (StateParameter::Z, false), - (StateParameter::VX, false), - (StateParameter::VY, false), - (StateParameter::VZ, false), - (StateParameter::FuelMass, false), - ]; - - let file = File::open(&self.path).context(StdIOSnafu { - action: "opening output trajectory file", - })?; - - let builder = ParquetRecordBatchReaderBuilder::try_new(file).unwrap(); - - let reader = builder.build().context(ParquetSnafu { - action: "building output trajectory file", - })?; - - for field in &reader.schema().fields { - if field.name().as_str() == "Epoch (UTC)" { - has_epoch = true; - } else { - for potential_field in &mut found_fields { - if field.name() == potential_field.0.to_field(None).name() { - potential_field.1 = true; - if potential_field.0 != StateParameter::FuelMass { - if let Some(frame_info) = field.metadata().get("Frame") { - // Frame is expected to be serialized as Dhall. - match serde_dhall::from_str(frame_info).parse::() { - Err(e) => { - return Err(InputOutputError::ParseDhall { - data: frame_info.to_string(), - err: format!("{e}"), - }) - } - Ok(deser_frame) => frame = Some(deser_frame), - }; - } - } - break; - } - } - } - } - - ensure!( - has_epoch, - MissingDataSnafu { - which: "Epoch (UTC)" - } - ); - - ensure!( - frame.is_some(), - MissingDataSnafu { - which: "Frame in metadata" - } - ); - - for (field, exists) in found_fields.iter().take(found_fields.len() - 1) { - ensure!( - exists, - MissingDataSnafu { - which: format!("Missing `{}` field", field.to_field(None).name()) - } - ); - } - - let sc_compat = found_fields.last().unwrap().1; - - let expected_type = std::any::type_name::().split("::").last().unwrap(); - - if expected_type == "Spacecraft" { - ensure!( - sc_compat, - MissingDataSnafu { - which: format!( - "Missing `{}` field", - found_fields.last().unwrap().0.to_field(None).name() - ) - } - ); - } else if sc_compat { - // Not a spacecraft, remove the fuel mass - if let Some(last_field) = found_fields.last_mut() { - if last_field.0 == StateParameter::FuelMass && last_field.1 { - last_field.1 = false; - } - } - } - - // At this stage, we know that the measurement is valid and the conversion is supported. - let mut traj = Traj::default(); - - // Now convert each batch on the fly - for maybe_batch in reader { - let batch = maybe_batch.unwrap(); - - let epochs = batch - .column_by_name("Epoch (UTC)") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(); - - let mut shared_data = vec![]; - - for (field, _) in found_fields.iter().take(found_fields.len() - 1) { - shared_data.push( - batch - .column_by_name(field.to_field(None).name()) - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(), - ); - } - - if expected_type == "Spacecraft" { - // Read the fuel only if this is a spacecraft we're building - shared_data.push( - batch - .column_by_name("fuel_mass (kg)") - .unwrap() - .as_any() - .downcast_ref::() - .unwrap(), - ); - } - - // Grab the frame -- it should have been serialized with all of the data so we don't need to reload it. - - // Build the states - for i in 0..batch.num_rows() { - let mut state = S::zeros(); - state.set_epoch(Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { - InputOutputError::Inconsistency { - msg: format!("{e} when parsing epoch"), - } - })?); - state.set_frame(frame.unwrap()); // We checked it was set above with an ensure! call - state.unset_stm(); // We don't have any STM data, so let's unset this. - - for (j, (param, exists)) in found_fields.iter().enumerate() { - if *exists { - state.set_value(*param, shared_data[j].value(i)).unwrap(); - } - } - - traj.states.push(state); - } - } - - // Remove any duplicates that may exist in the imported trajectory. - traj.finalize(); - - Ok(traj) - } - - fn repr(&self) -> Vec { - let mut r = Vec::new(); - r.push(format!("File: {}", self.path)); - for (k, v) in &self.metadata { - if k != "devices" { - r.push(format!("{k}: {v}")); - } - } - r - } -} - -impl Display for TrajectoryLoader { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - for item in self.repr() { - writeln!(f, "{item}")?; - } - Ok(()) - } -} - -#[cfg(feature = "python")] -#[pymethods] -impl TrajectoryLoader { - /// Initializes a new dynamic trajectory from the provided file, and the format kind - #[new] - #[pyo3( - text_signature = "(path, format='parquet', parquet_path=None, spacecraft_template=None)" - )] - fn new( - path: String, - format: Option, - parquet_path: Option, - spacecraft_template: Option, - ) -> Result { - if format.is_none() { - Self::from_parquet(path).map_err(|e| NyxError::CustomError { msg: e.to_string() }) - } else { - let uformat = format.unwrap().to_lowercase(); - match uformat.as_str() { - "parquet" => Self::from_parquet(path) - .map_err(|e| NyxError::CustomError { msg: e.to_string() }), - "oem" => { - // Now we check that everything is set correctly. - if parquet_path.is_none() { - return Err(NyxError::CustomError { - msg: "Loading an OEM requires `parquet_path` parameter for output file" - .to_string(), - }); - } - let sc_tpl = match spacecraft_template { - Some(sc) => sc, - None => { - warn!("No spacecraft template specified on OEM loading, assuming zero fuel mass"); - Spacecraft::default() - } - }; - - let traj = Traj::::from_oem_file(path, sc_tpl)?; - let out_pq = parquet_path.unwrap(); - // Convert to parquet - traj.to_parquet_simple(&out_pq) - .map_err(|e| NyxError::CustomError { msg: e.to_string() })?; - // Return Self with this path - Self::from_parquet(out_pq) - .map_err(|e| NyxError::CustomError { msg: e.to_string() }) - } - &_ => Err(NyxError::CustomError { - msg: format!("Unexpected format `{uformat}`"), - }), - } - } - } - - /// Converts this loaded trajectory into an Orbit trajectory (no spacecraft data) - fn to_orbit_traj(&self) -> Result { - Ok(OrbitTrajPy { - inner: self - .to_traj() - .map_err(|e| NyxError::CustomError { msg: e.to_string() })?, - }) - } - - /// Converts this loaded trajectory into an Orbit trajectory (no spacecraft data) - fn to_spacecraft_traj(&self) -> Result { - Ok(ScTrajPy { - inner: self - .to_traj() - .map_err(|e| NyxError::CustomError { msg: e.to_string() })?, - }) - } - - fn __repr__(&self) -> String { - format!("{self}") - } - - fn __getnewargs__(&self) -> Result<(String,), NyxError> { - Ok((self.path.clone(),)) - } - - #[cfg(feature = "python")] - fn __richcmp__(&self, other: &Self, op: CompareOp) -> Result { - match op { - CompareOp::Eq => Ok(self == other), - CompareOp::Ne => Ok(self != other), - _ => Err(PythonError::OperationError { op }), - } - } -} diff --git a/src/lib.rs b/src/lib.rs index 26b0c6f2..82dbdfeb 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -76,9 +76,6 @@ pub mod linalg { /// Re-export some useful things pub use self::cosmic::{Orbit, Spacecraft, State, TimeTagged}; -// #[cfg(feature = "python")] -// mod python; - /// The GMAT Earth gravitation parameter, used only for testing. #[cfg(test)] pub(crate) const GMAT_EARTH_GM: f64 = 398_600.441_5; diff --git a/src/mc/multivariate.rs b/src/mc/multivariate.rs index fd8c94b8..1ee9f509 100644 --- a/src/mc/multivariate.rs +++ b/src/mc/multivariate.rs @@ -141,7 +141,7 @@ impl MvnSpacecraft { StateParameter::Cd => { cov[(8, 8)] = disp.mean.unwrap_or(0.0).powi(2); } - StateParameter::DryMass | StateParameter::FuelMass => { + StateParameter::DryMass | StateParameter::PropMass => { cov[(9, 9)] = disp.mean.unwrap_or(0.0).powi(2); } _ => return Err(Box::new(StateError::ReadOnly { param: disp.param })), @@ -249,7 +249,7 @@ impl MvnSpacecraft { .std_dev(cov[(7, 7)]) .build(), StateDispersion::builder() - .param(StateParameter::FuelMass) + .param(StateParameter::PropMass) .std_dev(cov[(8, 8)]) .build(), ]; @@ -278,11 +278,11 @@ impl Distribution> for MvnSpacecraft { } else if coord < 6 { state.orbit.velocity_km_s[coord % 3] += val; } else if coord == 6 { - state.srp.cr += val; + state.srp.coeff_reflectivity += val; } else if coord == 7 { - state.drag.cd += val; + state.drag.coeff_drag += val; } else if coord == 8 { - state.fuel_mass_kg += val; + state.mass.prop_mass_kg += val; } } diff --git a/src/md/events/evaluators.rs b/src/md/events/evaluators.rs index 028d490b..b60e5d49 100644 --- a/src/md/events/evaluators.rs +++ b/src/md/events/evaluators.rs @@ -61,7 +61,7 @@ impl EventEvaluator for Event { state.orbit.ta_deg().context(EventPhysicsSnafu)?, 180.0, )), - StateParameter::FuelMass => Ok(state.fuel_mass_kg - self.desired_value), + StateParameter::PropMass => Ok(state.mass.prop_mass_kg - self.desired_value), _ => Ok(state.value(self.parameter).context(EventStateSnafu { param: self.parameter, })? - self.desired_value), diff --git a/src/md/events/mod.rs b/src/md/events/mod.rs index 545958eb..fffdc1c2 100644 --- a/src/md/events/mod.rs +++ b/src/md/events/mod.rs @@ -27,8 +27,7 @@ use crate::time::{Duration, Unit}; use crate::State; use anise::prelude::{Almanac, Frame}; use anise::structure::planetocentric::ellipsoid::Ellipsoid; -#[cfg(feature = "python")] -use pyo3::prelude::*; + use std::default::Default; use std::fmt; use std::sync::Arc; @@ -61,7 +60,7 @@ where /// Defines a state parameter event finder #[derive(Clone, Debug)] -#[cfg_attr(feature = "python", pyclass)] + pub struct Event { /// The state parameter pub parameter: StateParameter, diff --git a/src/md/mod.rs b/src/md/mod.rs index 7045d5b7..883601c5 100644 --- a/src/md/mod.rs +++ b/src/md/mod.rs @@ -27,7 +27,7 @@ pub mod prelude { pub use super::{ targeter::*, trajectory::{ExportCfg, Interpolatable, Traj}, - Event, ScTraj, StateParameter, + Event, StateParameter, Trajectory, }; pub use crate::cosmic::{try_achieve_b_plane, BPlane, BPlaneTarget, GuidanceMode, OrbitDual}; pub use crate::dynamics::{ @@ -53,8 +53,7 @@ pub use events::{Event, EventEvaluator}; pub mod objective; pub mod opti; pub use opti::targeter; -pub type ScTraj = trajectory::Traj; -// pub type Ephemeris = trajectory::Traj; +pub type Trajectory = trajectory::Traj; mod param; pub use param::StateParameter; diff --git a/src/md/opti/convert_impulsive.rs b/src/md/opti/convert_impulsive.rs index 9bcc97af..fec0a4d7 100644 --- a/src/md/opti/convert_impulsive.rs +++ b/src/md/opti/convert_impulsive.rs @@ -186,7 +186,6 @@ impl<'a, E: ErrorCtrl> Optimizer<'a, E, 3, 6> { let width = f64::from(max_obj_val).log10() as usize + 2 + max_obj_tol; - // let start_instant = Instant::now(); let max_iter = 10; for it in 0..=max_iter { diff --git a/src/md/opti/minimize_lm.rs b/src/md/opti/minimize_lm.rs index 936db988..223d7216 100644 --- a/src/md/opti/minimize_lm.rs +++ b/src/md/opti/minimize_lm.rs @@ -60,7 +60,6 @@ where /// The control solution to this problem. pub control: SVector, pub residuals: SVector, - // pub jacobian: SMatrix, pub jacobian: SMatrix, } diff --git a/src/md/opti/mod.rs b/src/md/opti/mod.rs index 51485570..f518084e 100644 --- a/src/md/opti/mod.rs +++ b/src/md/opti/mod.rs @@ -16,7 +16,6 @@ along with this program. If not, see . */ -// pub mod convert_impulsive; pub mod multipleshooting; pub use multipleshooting::{ctrlnodes, multishoot}; /// Uses a [Newton Raphson](https://en.wikipedia.org/wiki/Newton%27s_method_in_optimization) method where the Jacobian is computed via finite differencing. diff --git a/src/md/opti/multipleshooting/equidistant_heuristic.rs b/src/md/opti/multipleshooting/equidistant_heuristic.rs index ffa6f729..494da448 100644 --- a/src/md/opti/multipleshooting/equidistant_heuristic.rs +++ b/src/md/opti/multipleshooting/equidistant_heuristic.rs @@ -41,7 +41,7 @@ impl<'a> MultipleShooting<'a, Node, 3, 3> { // Compute the direction of the objective let mut direction = xf.radius_km - x0.orbit.radius_km; - if direction.norm() < 2e-16 { + if direction.norm() < f64::EPSILON { return Err(TargetingError::TargetsTooClose); } let distance_increment = direction.norm() / (node_count as f64); diff --git a/src/md/opti/multipleshooting/multishoot.rs b/src/md/opti/multipleshooting/multishoot.rs index 9e4145d0..efb5c1ed 100644 --- a/src/md/opti/multipleshooting/multishoot.rs +++ b/src/md/opti/multipleshooting/multishoot.rs @@ -352,7 +352,7 @@ impl, const O: usize> MultipleShootingSolution { &self, prop: &Propagator, almanac: Arc, - ) -> Result, MultipleShootingError> { + ) -> Result, MultipleShootingError> { let mut trajz = Vec::with_capacity(self.nodes.len()); for (i, node) in self.nodes.iter().copied().enumerate() { diff --git a/src/md/opti/raphson_finite_diff.rs b/src/md/opti/raphson_finite_diff.rs index 288991c9..620398c6 100644 --- a/src/md/opti/raphson_finite_diff.rs +++ b/src/md/opti/raphson_finite_diff.rs @@ -19,7 +19,7 @@ use super::solution::TargeterSolution; use super::targeter::Targeter; use crate::cosmic::{AstroAlmanacSnafu, AstroPhysicsSnafu}; -use crate::dynamics::guidance::{GuidanceError, LocalFrame, Mnvr}; +use crate::dynamics::guidance::{GuidanceError, LocalFrame, Maneuver, MnvrRepr}; use crate::errors::TargetingError; use crate::linalg::{SMatrix, SVector, Vector6}; use crate::md::{prelude::*, AstroSnafu, GuidanceSnafu, UnderdeterminedProblemSnafu}; @@ -71,12 +71,14 @@ impl Targeter<'_, V, O> { // Store the total correction in Vector3 let mut total_correction = SVector::::zeros(); - let mut mnvr = Mnvr { + let mut mnvr = Maneuver { start: correction_epoch, end: achievement_epoch, thrust_prct: 1.0, - alpha_inplane_radians: CommonPolynomial::Quadratic(0.0, 0.0, 0.0), - delta_outofplane_radians: CommonPolynomial::Quadratic(0.0, 0.0, 0.0), + representation: MnvrRepr::Angles { + azimuth: CommonPolynomial::Quadratic(0.0, 0.0, 0.0), + elevation: CommonPolynomial::Quadratic(0.0, 0.0, 0.0), + }, frame: LocalFrame::RCN, }; @@ -113,16 +115,26 @@ impl Targeter<'_, V, O> { Vary::EndEpoch => mnvr.end += var.init_guess.seconds(), Vary::StartEpoch => mnvr.start += var.init_guess.seconds(), Vary::MnvrAlpha | Vary::MnvrAlphaDot | Vary::MnvrAlphaDDot => { - mnvr.alpha_inplane_radians = mnvr - .alpha_inplane_radians - .add_val_in_order(var.init_guess, var.component.vec_index()) - .unwrap(); + match mnvr.representation { + MnvrRepr::Angles { azimuth, elevation } => { + let azimuth = azimuth + .add_val_in_order(var.init_guess, var.component.vec_index()) + .unwrap(); + mnvr.representation = MnvrRepr::Angles { azimuth, elevation }; + } + _ => unreachable!(), + }; } Vary::MnvrDelta | Vary::MnvrDeltaDot | Vary::MnvrDeltaDDot => { - mnvr.delta_outofplane_radians = mnvr - .delta_outofplane_radians - .add_val_in_order(var.init_guess, var.component.vec_index()) - .unwrap(); + match mnvr.representation { + MnvrRepr::Angles { azimuth, elevation } => { + let elevation = elevation + .add_val_in_order(var.init_guess, var.component.vec_index()) + .unwrap(); + mnvr.representation = MnvrRepr::Angles { azimuth, elevation }; + } + _ => unreachable!(), + }; } Vary::ThrustX | Vary::ThrustY | Vary::ThrustZ => { let mut vector = mnvr.direction(); @@ -328,16 +340,28 @@ impl Targeter<'_, V, O> { } } Vary::MnvrAlpha | Vary::MnvrAlphaDot | Vary::MnvrAlphaDDot => { - this_mnvr.alpha_inplane_radians = mnvr - .alpha_inplane_radians - .add_val_in_order(pert, var.component.vec_index()) - .unwrap(); + match mnvr.representation { + MnvrRepr::Angles { azimuth, elevation } => { + let azimuth = azimuth + .add_val_in_order(pert, var.component.vec_index()) + .unwrap(); + this_mnvr.representation = + MnvrRepr::Angles { azimuth, elevation }; + } + _ => unreachable!(), + }; } Vary::MnvrDelta | Vary::MnvrDeltaDot | Vary::MnvrDeltaDDot => { - this_mnvr.delta_outofplane_radians = mnvr - .delta_outofplane_radians - .add_val_in_order(pert, var.component.vec_index()) - .unwrap(); + match mnvr.representation { + MnvrRepr::Angles { azimuth, elevation } => { + let elevation = elevation + .add_val_in_order(pert, var.component.vec_index()) + .unwrap(); + this_mnvr.representation = + MnvrRepr::Angles { azimuth, elevation }; + } + _ => unreachable!(), + }; } Vary::ThrustX | Vary::ThrustY | Vary::ThrustZ => { let mut vector = this_mnvr.direction(); @@ -615,16 +639,26 @@ impl Targeter<'_, V, O> { } } Vary::MnvrAlpha | Vary::MnvrAlphaDot | Vary::MnvrAlphaDDot => { - mnvr.alpha_inplane_radians = mnvr - .alpha_inplane_radians - .add_val_in_order(corr, var.component.vec_index()) - .unwrap(); + match mnvr.representation { + MnvrRepr::Angles { azimuth, elevation } => { + let azimuth = azimuth + .add_val_in_order(corr, var.component.vec_index()) + .unwrap(); + mnvr.representation = MnvrRepr::Angles { azimuth, elevation }; + } + _ => unreachable!(), + }; } Vary::MnvrDelta | Vary::MnvrDeltaDot | Vary::MnvrDeltaDDot => { - mnvr.delta_outofplane_radians = mnvr - .delta_outofplane_radians - .add_val_in_order(corr, var.component.vec_index()) - .unwrap(); + match mnvr.representation { + MnvrRepr::Angles { azimuth, elevation } => { + let elevation = elevation + .add_val_in_order(corr, var.component.vec_index()) + .unwrap(); + mnvr.representation = MnvrRepr::Angles { azimuth, elevation }; + } + _ => unreachable!(), + }; } Vary::ThrustX | Vary::ThrustY | Vary::ThrustZ => { let mut vector = mnvr.direction(); diff --git a/src/md/opti/solution.rs b/src/md/opti/solution.rs index b072f083..072e0aca 100644 --- a/src/md/opti/solution.rs +++ b/src/md/opti/solution.rs @@ -19,7 +19,7 @@ use hifitime::TimeUnits; use snafu::{ensure, ResultExt}; -use crate::dynamics::guidance::{LocalFrame, Mnvr}; +use crate::dynamics::guidance::{LocalFrame, Maneuver, MnvrRepr}; use crate::linalg::SVector; use crate::md::objective::Objective; use crate::md::{prelude::*, GuidanceSnafu, NotFiniteSnafu, TargetingError}; @@ -61,17 +61,19 @@ impl TargeterSolution { } /// Returns a maneuver if targeter solution was a finite burn maneuver - pub fn to_mnvr(&self) -> Result { + pub fn to_mnvr(&self) -> Result { ensure!(self.is_finite_burn(), NotFiniteSnafu); let correction_epoch = self.corrected_state.epoch(); let achievement_epoch = self.achieved_state.epoch(); - let mut mnvr = Mnvr { + let mut mnvr = Maneuver { start: correction_epoch, end: achievement_epoch, thrust_prct: 1.0, - alpha_inplane_radians: CommonPolynomial::Quadratic(0.0, 0.0, 0.0), - delta_outofplane_radians: CommonPolynomial::Quadratic(0.0, 0.0, 0.0), + representation: MnvrRepr::Angles { + azimuth: CommonPolynomial::Quadratic(0.0, 0.0, 0.0), + elevation: CommonPolynomial::Quadratic(0.0, 0.0, 0.0), + }, frame: LocalFrame::RCN, }; @@ -109,16 +111,26 @@ impl TargeterSolution { } } Vary::MnvrAlpha | Vary::MnvrAlphaDot | Vary::MnvrAlphaDDot => { - mnvr.alpha_inplane_radians = mnvr - .alpha_inplane_radians - .add_val_in_order(corr, var.component.vec_index()) - .unwrap(); + match mnvr.representation { + MnvrRepr::Angles { azimuth, elevation } => { + let azimuth = azimuth + .add_val_in_order(corr, var.component.vec_index()) + .unwrap(); + mnvr.representation = MnvrRepr::Angles { azimuth, elevation }; + } + _ => unreachable!(), + }; } Vary::MnvrDelta | Vary::MnvrDeltaDot | Vary::MnvrDeltaDDot => { - mnvr.delta_outofplane_radians = mnvr - .delta_outofplane_radians - .add_val_in_order(corr, var.component.vec_index()) - .unwrap(); + match mnvr.representation { + MnvrRepr::Angles { azimuth, elevation } => { + let elevation = elevation + .add_val_in_order(corr, var.component.vec_index()) + .unwrap(); + mnvr.representation = MnvrRepr::Angles { azimuth, elevation }; + } + _ => unreachable!(), + }; } Vary::ThrustX | Vary::ThrustY | Vary::ThrustZ => { let mut vector = mnvr.direction(); diff --git a/src/md/param.rs b/src/md/param.rs index 311d8fd8..322ecc2f 100644 --- a/src/md/param.rs +++ b/src/md/param.rs @@ -20,15 +20,14 @@ use super::NyxError; use arrow::datatypes::{DataType, Field}; use core::fmt; use enum_iterator::Sequence; -#[cfg(feature = "python")] -use pyo3::prelude::*; + use serde::{Deserialize, Serialize}; use std::{collections::HashMap, str::FromStr}; /// Common state parameters #[allow(non_camel_case_types, clippy::upper_case_acronyms)] #[derive(Copy, Clone, Debug, PartialEq, Sequence, Serialize, Deserialize)] -#[cfg_attr(feature = "python", pyclass)] + pub enum StateParameter { /// Argument of Latitude (deg) AoL, @@ -64,8 +63,6 @@ pub enum StateParameter { Energy, /// Flight path angle (deg) FlightPathAngle, - /// fuel mass in kilograms - FuelMass, /// Geodetic height (km) Height, /// Geodetic latitude (deg) @@ -96,6 +93,8 @@ pub enum StateParameter { PeriapsisRadius, /// Orbital period (s) Period, + /// prop mass in kilograms + PropMass, /// Right ascension (deg) RightAscension, /// Right ascension of the ascending node (deg) @@ -110,6 +109,8 @@ pub enum StateParameter { SemiMinorAxis, /// Thrust (Newtons) Thrust, + /// Total mass + TotalMass, /// True anomaly TrueAnomaly, /// True longitude @@ -132,7 +133,6 @@ pub enum StateParameter { VZ, } -#[cfg_attr(feature = "python", pymethods)] impl StateParameter { /// Returns the default event finding precision in the unit of that parameter pub fn default_event_precision(&self) -> f64 { @@ -182,7 +182,7 @@ impl StateParameter { // Special Self::Energy => 1e-3, - Self::DryMass | Self::FuelMass => 1e-3, + Self::DryMass | Self::PropMass => 1e-3, Self::Period => 1e-1, _ => unimplemented!("{self} cannot be used for event finding"), } @@ -203,7 +203,7 @@ impl StateParameter { matches!( &self, Self::DryMass - | Self::FuelMass + | Self::PropMass | Self::Cr | Self::Cd | Self::Isp @@ -256,24 +256,12 @@ impl StateParameter { Self::C3 | Self::Energy => "km^2/s^2", - Self::DryMass | Self::FuelMass => "kg", + Self::DryMass | Self::PropMass => "kg", Self::Isp => "isp", Self::Thrust => "N", _ => "", } } - - /// Prints this orbit in Keplerian form - #[cfg(feature = "python")] - fn __str__(&self) -> String { - format!("{self}") - } - - #[cfg(feature = "python")] - #[new] - fn py_new(name: String) -> Result { - Self::from_str(&name) - } } impl StateParameter { @@ -339,7 +327,6 @@ impl FromStr for StateParameter { "ecc" => Ok(Self::Eccentricity), "energy" => Ok(Self::Energy), "fpa" => Ok(Self::FlightPathAngle), - "fuel_mass" => Ok(Self::FuelMass), "guidance_mode" | "mode" => Ok(Self::GuidanceMode), "geodetic_height" => Ok(Self::Height), "geodetic_latitude" => Ok(Self::Latitude), @@ -354,6 +341,7 @@ impl FromStr for StateParameter { "ma" => Ok(Self::MeanAnomaly), "periapsis_radius" => Ok(Self::PeriapsisRadius), "period" => Ok(Self::Period), + "prop_mass" => Ok(Self::PropMass), "right_asc" => Ok(Self::RightAscension), "raan" => Ok(Self::RAAN), "rmag" => Ok(Self::Rmag), @@ -363,6 +351,7 @@ impl FromStr for StateParameter { "ta" => Ok(Self::TrueAnomaly), "tlong" => Ok(Self::TrueLongitude), "thrust" => Ok(Self::Thrust), + "total_mass" => Ok(Self::TotalMass), "vdeclin" => Ok(Self::VelocityDeclination), "vmag" => Ok(Self::Vmag), "x" => Ok(Self::X), @@ -399,7 +388,6 @@ impl fmt::Display for StateParameter { Self::Eccentricity => "ecc", Self::Energy => "energy", Self::FlightPathAngle => "fpa", - Self::FuelMass => "fuel_mass", Self::GuidanceMode => "guidance_mode", Self::Height => "geodetic_height", Self::Latitude => "geodetic_latitude", @@ -414,6 +402,7 @@ impl fmt::Display for StateParameter { Self::MeanAnomaly => "ma", Self::PeriapsisRadius => "periapsis_radius", Self::Period => "period", + Self::PropMass => "prop_mass", Self::RightAscension => "right_asc", Self::RAAN => "raan", Self::Rmag => "rmag", @@ -421,6 +410,7 @@ impl fmt::Display for StateParameter { Self::SemiMinorAxis => "semi_minor", Self::SMA => "sma", Self::Thrust => "thrust", + Self::TotalMass => "total_mass", Self::TrueAnomaly => "ta", Self::TrueLongitude => "tlong", Self::VelocityDeclination => "vdeclin", @@ -465,7 +455,6 @@ mod ut_state_param { StateParameter::Eccentricity, StateParameter::Energy, StateParameter::FlightPathAngle, - StateParameter::FuelMass, StateParameter::GuidanceMode, StateParameter::Height, StateParameter::Latitude, @@ -480,6 +469,7 @@ mod ut_state_param { StateParameter::MeanAnomaly, StateParameter::PeriapsisRadius, StateParameter::Period, + StateParameter::PropMass, StateParameter::RightAscension, StateParameter::RAAN, StateParameter::Rmag, @@ -487,6 +477,7 @@ mod ut_state_param { StateParameter::SemiMinorAxis, StateParameter::SMA, StateParameter::Thrust, + StateParameter::TotalMass, StateParameter::TrueAnomaly, StateParameter::TrueLongitude, StateParameter::VelocityDeclination, diff --git a/src/md/trajectory/interpolatable.rs b/src/md/trajectory/interpolatable.rs index b845cd5b..5c116698 100644 --- a/src/md/trajectory/interpolatable.rs +++ b/src/md/trajectory/interpolatable.rs @@ -97,10 +97,10 @@ impl Interpolatable for Spacecraft { // Fuel is linearly interpolated -- should really be a Lagrange interpolation here let first = states.first().unwrap(); let last = states.last().unwrap(); - let fuel_kg_dt = - (last.fuel_mass_kg - first.fuel_mass_kg) / (last.epoch() - first.epoch()).to_seconds(); + let prop_kg_dt = (last.mass.prop_mass_kg - first.mass.prop_mass_kg) + / (last.epoch() - first.epoch()).to_seconds(); - self.fuel_mass_kg += fuel_kg_dt * (epoch - first.epoch()).to_seconds(); + self.mass.prop_mass_kg += prop_kg_dt * (epoch - first.epoch()).to_seconds(); Ok(self) } diff --git a/src/md/trajectory/sc_traj.rs b/src/md/trajectory/sc_traj.rs index 794d134b..05bd0ad8 100644 --- a/src/md/trajectory/sc_traj.rs +++ b/src/md/trajectory/sc_traj.rs @@ -20,15 +20,19 @@ use anise::astro::Aberration; use anise::constants::orientations::J2000; use anise::errors::AlmanacError; use anise::prelude::{Almanac, Frame, Orbit}; +use arrow::array::RecordBatchReader; +use arrow::array::{Float64Array, StringArray}; use hifitime::TimeSeries; -use snafu::ResultExt; +use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder; +use snafu::{ensure, ResultExt}; use super::TrajError; use super::{ExportCfg, Traj}; use crate::cosmic::Spacecraft; use crate::errors::{FromAlmanacSnafu, NyxError}; use crate::io::watermark::prj_name_ver; -use crate::md::prelude::StateParameter; +use crate::io::{InputOutputError, MissingDataSnafu, ParquetSnafu, StdIOSnafu}; +use crate::md::prelude::{Interpolatable, StateParameter}; use crate::md::EventEvaluator; use crate::time::{Duration, Epoch, Format, Formatter, TimeUnits}; use crate::State; @@ -121,25 +125,6 @@ impl Traj { Ok(traj) } - /// A shortcut to `to_parquet_with_cfg` - pub fn to_parquet_with_step>( - &self, - path: P, - step: Duration, - almanac: Arc, - ) -> Result<(), Box> { - self.to_parquet_with_cfg( - path, - ExportCfg { - step: Some(step), - ..Default::default() - }, - almanac, - )?; - - Ok(()) - } - /// Exports this trajectory to the provided filename in parquet format with only the epoch, the geodetic latitude, longitude, and height at one state per minute. /// Must provide a body fixed frame to correctly compute the latitude and longitude. #[allow(clippy::identity_op)] @@ -448,6 +433,194 @@ impl Traj { ); Ok(path_buf) } + + pub fn from_parquet>(path: P) -> Result { + let file = File::open(&path).context(StdIOSnafu { + action: "opening trajectory file", + })?; + + let builder = ParquetRecordBatchReaderBuilder::try_new(file).unwrap(); + + let mut metadata = HashMap::new(); + // Build the custom metadata + if let Some(file_metadata) = builder.metadata().file_metadata().key_value_metadata() { + for key_value in file_metadata { + if !key_value.key.starts_with("ARROW:") { + metadata.insert( + key_value.key.clone(), + key_value.value.clone().unwrap_or("[unset]".to_string()), + ); + } + } + } + + // Check the schema + let mut has_epoch = false; // Required + let mut frame = None; + + let mut found_fields = vec![ + (StateParameter::X, false), + (StateParameter::Y, false), + (StateParameter::Z, false), + (StateParameter::VX, false), + (StateParameter::VY, false), + (StateParameter::VZ, false), + (StateParameter::PropMass, false), + ]; + + let file = File::open(path).context(StdIOSnafu { + action: "opening output trajectory file", + })?; + + let builder = ParquetRecordBatchReaderBuilder::try_new(file).unwrap(); + + let reader = builder.build().context(ParquetSnafu { + action: "building output trajectory file", + })?; + + for field in &reader.schema().fields { + if field.name().as_str() == "Epoch (UTC)" { + has_epoch = true; + } else { + for potential_field in &mut found_fields { + if field.name() == potential_field.0.to_field(None).name() { + potential_field.1 = true; + if potential_field.0 != StateParameter::PropMass { + if let Some(frame_info) = field.metadata().get("Frame") { + // Frame is expected to be serialized as Dhall. + match serde_dhall::from_str(frame_info).parse::() { + Err(e) => { + return Err(InputOutputError::ParseDhall { + data: frame_info.to_string(), + err: format!("{e}"), + }) + } + Ok(deser_frame) => frame = Some(deser_frame), + }; + } + } + break; + } + } + } + } + + ensure!( + has_epoch, + MissingDataSnafu { + which: "Epoch (UTC)" + } + ); + + ensure!( + frame.is_some(), + MissingDataSnafu { + which: "Frame in metadata" + } + ); + + for (field, exists) in found_fields.iter().take(found_fields.len() - 1) { + ensure!( + exists, + MissingDataSnafu { + which: format!("Missing `{}` field", field.to_field(None).name()) + } + ); + } + + let sc_compat = found_fields.last().unwrap().1; + + let expected_type = std::any::type_name::() + .split("::") + .last() + .unwrap(); + + if expected_type == "Spacecraft" { + ensure!( + sc_compat, + MissingDataSnafu { + which: format!( + "Missing `{}` field", + found_fields.last().unwrap().0.to_field(None).name() + ) + } + ); + } else if sc_compat { + // Not a spacecraft, remove the prop mass + if let Some(last_field) = found_fields.last_mut() { + if last_field.0 == StateParameter::PropMass && last_field.1 { + last_field.1 = false; + } + } + } + + // At this stage, we know that the measurement is valid and the conversion is supported. + let mut traj = Traj::default(); + + // Now convert each batch on the fly + for maybe_batch in reader { + let batch = maybe_batch.unwrap(); + + let epochs = batch + .column_by_name("Epoch (UTC)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(); + + let mut shared_data = vec![]; + + for (field, _) in found_fields.iter().take(found_fields.len() - 1) { + shared_data.push( + batch + .column_by_name(field.to_field(None).name()) + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(), + ); + } + + if expected_type == "Spacecraft" { + // Read the prop only if this is a spacecraft we're building + shared_data.push( + batch + .column_by_name("prop_mass (kg)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(), + ); + } + + // Grab the frame -- it should have been serialized with all of the data so we don't need to reload it. + + // Build the states + for i in 0..batch.num_rows() { + let mut state = Spacecraft::zeros(); + state.set_epoch(Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { + InputOutputError::Inconsistency { + msg: format!("{e} when parsing epoch"), + } + })?); + state.set_frame(frame.unwrap()); // We checked it was set above with an ensure! call + state.unset_stm(); // We don't have any STM data, so let's unset this. + + for (j, (param, exists)) in found_fields.iter().enumerate() { + if *exists { + state.set_value(*param, shared_data[j].value(i)).unwrap(); + } + } + + traj.states.push(state); + } + } + + // Remove any duplicates that may exist in the imported trajectory. + traj.finalize(); + + Ok(traj) + } } #[cfg(test)] diff --git a/src/md/trajectory/traj.rs b/src/md/trajectory/traj.rs index 01a2c302..37152a62 100644 --- a/src/md/trajectory/traj.rs +++ b/src/md/trajectory/traj.rs @@ -166,6 +166,25 @@ where self.to_parquet(path, None, cfg, almanac) } + /// A shortcut to `to_parquet_with_cfg` + pub fn to_parquet_with_step>( + &self, + path: P, + step: Duration, + almanac: Arc, + ) -> Result<(), Box> { + self.to_parquet_with_cfg( + path, + ExportCfg { + step: Some(step), + ..Default::default() + }, + almanac, + )?; + + Ok(()) + } + /// Store this trajectory arc to a parquet file with the provided configuration and event evaluators pub fn to_parquet>( &self, diff --git a/src/od/estimate/kfestimate.rs b/src/od/estimate/kfestimate.rs index b141e95d..495e6ebe 100644 --- a/src/od/estimate/kfestimate.rs +++ b/src/od/estimate/kfestimate.rs @@ -128,9 +128,15 @@ impl KfEstimate { (3.0 * delta_orbit.velocity_km_s.x.abs()).powi(2), (3.0 * delta_orbit.velocity_km_s.y.abs()).powi(2), (3.0 * delta_orbit.velocity_km_s.z.abs()).powi(2), - (3.0 * (nominal_state.srp.cr - dispersed_state.state.srp.cr).abs()).powi(2), - (3.0 * (nominal_state.drag.cd - dispersed_state.state.drag.cd).abs()).powi(2), - (3.0 * (nominal_state.fuel_mass_kg - dispersed_state.state.fuel_mass_kg).abs()).powi(2), + (3.0 * (nominal_state.srp.coeff_reflectivity + - dispersed_state.state.srp.coeff_reflectivity) + .abs()) + .powi(2), + (3.0 * (nominal_state.drag.coeff_drag - dispersed_state.state.drag.coeff_drag).abs()) + .powi(2), + (3.0 * (nominal_state.mass.prop_mass_kg - dispersed_state.state.mass.prop_mass_kg) + .abs()) + .powi(2), ]; let diag = OVector::>::from_iterator(diag_data); diff --git a/src/od/estimate/sc_uncertainty.rs b/src/od/estimate/sc_uncertainty.rs index 99fa3aaf..740a54c5 100644 --- a/src/od/estimate/sc_uncertainty.rs +++ b/src/od/estimate/sc_uncertainty.rs @@ -50,9 +50,9 @@ pub struct SpacecraftUncertainty { #[builder(default = 50e-5)] pub vz_km_s: f64, #[builder(default)] - pub cr: f64, + pub coeff_reflectivity: f64, #[builder(default)] - pub cd: f64, + pub coeff_drag: f64, #[builder(default)] pub mass_kg: f64, } @@ -68,8 +68,8 @@ impl SpacecraftUncertainty { || self.vx_km_s < 0.0 || self.vy_km_s < 0.0 || self.vz_km_s < 0.0 - || self.cd < 0.0 - || self.cr < 0.0 + || self.coeff_drag < 0.0 + || self.coeff_reflectivity < 0.0 || self.mass_kg < 0.0 { return Err(PhysicsError::AppliedMath { @@ -104,8 +104,8 @@ impl SpacecraftUncertainty { 0.0, 0.0, 0.0, - self.cr.powi(2), - self.cd.powi(2), + self.coeff_reflectivity.powi(2), + self.coeff_drag.powi(2), self.mass_kg.powi(2), ])); @@ -154,7 +154,7 @@ impl fmt::Display for SpacecraftUncertainty { writeln!( f, "σ_cr = {} σ_cd = {} σ_mass = {} kg", - self.cr, self.cd, self.mass_kg + self.coeff_reflectivity, self.coeff_drag, self.mass_kg ) } } diff --git a/src/od/ground_station/mod.rs b/src/od/ground_station/mod.rs index 5ddbc476..7c24b857 100644 --- a/src/od/ground_station/mod.rs +++ b/src/od/ground_station/mod.rs @@ -38,13 +38,8 @@ pub mod builtin; pub mod event; pub mod trk_device; -#[cfg(feature = "python")] -use pyo3::prelude::*; - /// GroundStation defines a two-way ranging and doppler station. #[derive(Debug, Clone, Serialize, Deserialize, PartialEq)] -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.orbit_determination"))] pub struct GroundStation { pub name: String, /// in degrees diff --git a/src/od/msr/measurement.rs b/src/od/msr/measurement.rs index b3778c32..114f8e8c 100644 --- a/src/od/msr/measurement.rs +++ b/src/od/msr/measurement.rs @@ -69,7 +69,7 @@ impl Measurement { /// Returns a vector specifying which measurement types are available. pub fn availability(&self, types: &IndexSet) -> Vec { - let mut rtn = Vec::with_capacity(types.len()); + let mut rtn = vec![false; types.len()]; for (i, t) in types.iter().enumerate() { if self.data.contains_key(t) { rtn[i] = true; diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index d42688da..5d43fa8d 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -229,6 +229,11 @@ impl ScalarSensitivityT _tx: PhantomData::<_>, }) } + MeasurementType::ReceiveFrequency | MeasurementType::TransmitFrequency => { + Err(ODError::MeasurementSimError { + details: format!("{msr_type:?} is only supported in CCSDS TDM parsing"), + }) + } } } } diff --git a/src/od/msr/trackingdata/io_ccsds_tdm.rs b/src/od/msr/trackingdata/io_ccsds_tdm.rs index 11152483..8758ade6 100644 --- a/src/od/msr/trackingdata/io_ccsds_tdm.rs +++ b/src/od/msr/trackingdata/io_ccsds_tdm.rs @@ -20,10 +20,11 @@ use crate::io::watermark::prj_name_ver; use crate::io::ExportCfg; use crate::io::{InputOutputError, StdIOSnafu}; use crate::od::msr::{Measurement, MeasurementType}; +use anise::constants::SPEED_OF_LIGHT_KM_S; use hifitime::efmt::{Format, Formatter}; use hifitime::prelude::Epoch; use hifitime::TimeScale; -use indexmap::IndexMap; +use indexmap::{IndexMap, IndexSet}; use snafu::ResultExt; use std::collections::{BTreeMap, HashMap}; use std::fs::File; @@ -44,13 +45,18 @@ impl TrackingDataArc { action: "opening CCSDS TDM file for tracking arc", })?; + let source = path.as_ref().to_path_buf().display().to_string(); + info!("parsing CCSDS TDM {source}"); + let mut measurements = BTreeMap::new(); + let mut metadata = HashMap::new(); let reader = BufReader::new(file); let mut in_data_section = false; let mut current_tracker = String::new(); let mut time_system = TimeScale::UTC; + let mut has_freq_data = false; for line in reader.lines() { let line = line.context(StdIOSnafu { @@ -88,10 +94,27 @@ impl TrackingDataArc { } } } + + let mut splt = line.split('='); + if let Some(keyword) = splt.nth(0) { + // Get the zeroth item again since we've consumed the first zeroth one. + if let Some(value) = splt.nth(0) { + metadata.insert(keyword.trim().to_string(), value.trim().to_string()); + } + } + continue; } if let Some((mtype, epoch, value)) = parse_measurement_line(line, time_system)? { + if [ + MeasurementType::ReceiveFrequency, + MeasurementType::TransmitFrequency, + ] + .contains(&mtype) + { + has_freq_data = true; + } measurements .entry(epoch) .or_insert_with(|| Measurement { @@ -104,10 +127,127 @@ impl TrackingDataArc { } } - Ok(Self { + let mut turnaround_ratio = None; + let drop_freq_data; + if has_freq_data { + // If there is any frequency measurement, compute the turn-around ratio. + if let Some(ta_num_str) = metadata.get("TURNAROUND_NUMERATOR") { + if let Some(ta_denom_str) = metadata.get("TURNAROUND_DENOMINATOR") { + if let Ok(ta_num) = ta_num_str.parse::() { + if let Ok(ta_denom) = ta_denom_str.parse::() { + // turn-around ratio is set. + turnaround_ratio = Some(f64::from(ta_num) / f64::from(ta_denom)); + info!("turn-around ratio is {ta_num}/{ta_denom}"); + drop_freq_data = false; + } else { + error!("turn-around denominator `{ta_denom_str}` is not a valid double precision float"); + drop_freq_data = true; + } + } else { + error!("turn-around numerator `{ta_num_str}` is not a valid double precision float"); + drop_freq_data = true; + } + } else { + error!("required turn-around denominator missing from metadata -- dropping ALL RECEIVE/TRANSMIT data"); + drop_freq_data = true; + } + } else { + error!("required turn-around numerator missing from metadata -- dropping ALL RECEIVE/TRANSMIT data"); + drop_freq_data = true; + } + } else { + drop_freq_data = true; + } + + // Now, let's convert the receive and transmit frequencies to Doppler measurements in velocity units. + // We expect the transmit and receive frequencies to have the exact same timestamp. + let mut freq_types = IndexSet::new(); + freq_types.insert(MeasurementType::ReceiveFrequency); + freq_types.insert(MeasurementType::TransmitFrequency); + let mut latest_transmit_freq = None; + for (epoch, measurement) in measurements.iter_mut() { + if drop_freq_data { + for freq in &freq_types { + measurement.data.swap_remove(freq); + } + continue; + } + + let avail = measurement.availability(&freq_types); + let use_prev_transmit_freq; + let num_freq_msr = avail + .iter() + .copied() + .map(|v| if v { 1 } else { 0 }) + .sum::(); + if num_freq_msr == 0 { + // No frequency measurements + continue; + } else if num_freq_msr == 1 { + // avail[0] means that Receive Freq is available + // avail[1] means that Transmit Freq is available + // We can only compute Doppler data from one data point if that data point + // if the receive frequency and the transmit frequency was previously set. + if latest_transmit_freq.is_some() && avail[0] { + use_prev_transmit_freq = true; + warn!( + "no transmit frequency at {epoch}, using previous value of {} Hz", + latest_transmit_freq.unwrap() + ); + } else { + warn!("only one of receive or transmit frequencies found at {epoch}, ignoring"); + for freq in &freq_types { + measurement.data.swap_remove(freq); + } + continue; + } + } else { + use_prev_transmit_freq = false; + } + + if !use_prev_transmit_freq { + // Update the latest transmit frequency since it's set. + latest_transmit_freq = Some( + *measurement + .data + .get(&MeasurementType::TransmitFrequency) + .unwrap(), + ); + } + + let transmit_freq_hz = latest_transmit_freq.unwrap(); + let receive_freq_hz = *measurement + .data + .get(&MeasurementType::ReceiveFrequency) + .unwrap(); + + // Compute the Doppler shift, equation from section 3.5.2.8.2 of CCSDS TDM v2 specs + let doppler_shift_hz = transmit_freq_hz * turnaround_ratio.unwrap() - receive_freq_hz; + // Compute the expected Doppler measurement as range-rate. + let rho_dot_km_s = (doppler_shift_hz * SPEED_OF_LIGHT_KM_S) + / (2.0 * transmit_freq_hz * turnaround_ratio.unwrap()); + + // Finally, replace the frequency data with a Doppler measurement. + for freq in &freq_types { + measurement.data.swap_remove(freq); + } + measurement + .data + .insert(MeasurementType::Doppler, rho_dot_km_s); + } + + let trk = Self { measurements, - source: Some(path.as_ref().to_path_buf().display().to_string()), - }) + source: Some(source), + }; + + if trk.unique_types().is_empty() { + Err(InputOutputError::EmptyDataset { + action: "CCSDS TDM file", + }) + } else { + Ok(trk) + } } /// Store this tracking arc to a CCSDS TDM file, with optional metadata and a timestamp appended to the filename. @@ -254,6 +394,8 @@ impl TrackingDataArc { MeasurementType::Doppler => "DOPPLER_INTEGRATED", MeasurementType::Azimuth => "ANGLE_1", MeasurementType::Elevation => "ANGLE_2", + MeasurementType::ReceiveFrequency => "RECEIVE_FREQ", + MeasurementType::TransmitFrequency => "TRANSMIT_FREQ", }; writeln!( @@ -295,6 +437,10 @@ fn parse_measurement_line( "DOPPLER_INSTANTANEOUS" | "DOPPLER_INTEGRATED" => MeasurementType::Doppler, "ANGLE_1" => MeasurementType::Azimuth, "ANGLE_2" => MeasurementType::Elevation, + "RECEIVE_FREQ" | "RECEIVE_FREQ_1" | "RECEIVE_FREQ_2" | "RECEIVE_FREQ_3" + | "RECEIVE_FREQ_4" | "RECEIVE_FREQ_5" => MeasurementType::ReceiveFrequency, + "TRANSMIT_FREQ" | "TRANSMIT_FREQ_1" | "TRANSMIT_FREQ_2" | "TRANSMIT_FREQ_3" + | "TRANSMIT_FREQ_4" | "TRANSMIT_FREQ_5" => MeasurementType::TransmitFrequency, _ => { return Err(InputOutputError::UnsupportedData { which: mtype_str.to_string(), diff --git a/src/od/msr/trackingdata/io_parquet.rs b/src/od/msr/trackingdata/io_parquet.rs index fd55b0b8..89450c67 100644 --- a/src/od/msr/trackingdata/io_parquet.rs +++ b/src/od/msr/trackingdata/io_parquet.rs @@ -228,7 +228,7 @@ impl TrackingDataArc { ensure!( !self.is_empty(), EmptyDatasetSnafu { - action: "exporting tracking data arc" + action: "tracking data arc to parquet" } ); diff --git a/src/od/msr/types.rs b/src/od/msr/types.rs index 79c46059..2069dc34 100644 --- a/src/od/msr/types.rs +++ b/src/od/msr/types.rs @@ -33,6 +33,10 @@ pub enum MeasurementType { Azimuth, #[serde(rename = "elevation_deg")] Elevation, + #[serde(rename = "receive_freq")] + ReceiveFrequency, + #[serde(rename = "transmit_freq")] + TransmitFrequency, } impl MeasurementType { @@ -42,6 +46,7 @@ impl MeasurementType { Self::Range => "km", Self::Doppler => "km/s", Self::Azimuth | Self::Elevation => "deg", + Self::ReceiveFrequency | Self::TransmitFrequency => "Hz", } } @@ -66,6 +71,9 @@ impl MeasurementType { Self::Doppler => Ok(aer.range_rate_km_s + noise), Self::Azimuth => Ok(aer.azimuth_deg + noise), Self::Elevation => Ok(aer.elevation_deg + noise), + Self::ReceiveFrequency | Self::TransmitFrequency => Err(ODError::MeasurementSimError { + details: format!("{self:?} is only supported in CCSDS TDM parsing"), + }), } } @@ -94,6 +102,9 @@ impl MeasurementType { let el_deg = (aer_t1.elevation_deg + aer_t0.elevation_deg) * 0.5; Ok(el_deg + noise / 2.0_f64.sqrt()) } + Self::ReceiveFrequency | Self::TransmitFrequency => Err(ODError::MeasurementSimError { + details: format!("{self:?} is only supported in CCSDS TDM parsing"), + }), } } } diff --git a/src/od/noise/gauss_markov.rs b/src/od/noise/gauss_markov.rs index a004253a..8b7dc93b 100644 --- a/src/od/noise/gauss_markov.rs +++ b/src/od/noise/gauss_markov.rs @@ -17,20 +17,11 @@ */ use crate::io::{ConfigError, ConfigRepr}; -#[cfg(feature = "python")] -use crate::python::pyo3utils::pyany_to_value; use hifitime::{Duration, Epoch, TimeUnits}; -#[cfg(feature = "python")] -use pyo3::prelude::*; -#[cfg(feature = "python")] -use pyo3::types::{PyDict, PyList, PyType}; -#[cfg(feature = "python")] -use pythonize::{depythonize, pythonize}; + use rand::Rng; use rand_distr::Normal; use serde_derive::{Deserialize, Serialize}; -#[cfg(feature = "python")] -use std::collections::BTreeMap; use std::fmt; use std::ops::Mul; @@ -48,8 +39,6 @@ use super::Stochastics; /// /// s(t - t_0) = ((q * τ) / 2) * (1 - exp((-2 / τ) * (t - t_0))) #[derive(Copy, Clone, Debug, Serialize, Deserialize, PartialEq)] -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.orbit_determination"))] pub struct GaussMarkov { /// The time constant, tau gives the correlation time, or the time over which the intensity of the time correlation will fade to 1/e of its prior value. (This is sometimes incorrectly referred to as the "half-life" of the process.) pub tau: Duration, @@ -168,192 +157,6 @@ impl Mul for GaussMarkov { } } -#[cfg_attr(feature = "python", pymethods)] -impl GaussMarkov { - #[cfg(feature = "python")] - pub fn __repr__(&self) -> String { - format!("{self:?}") - } - - #[cfg(feature = "python")] - pub fn __str__(&self) -> String { - format!("{self}") - } - - #[cfg(feature = "python")] - #[new] - #[pyo3(text_signature = "(tau, sigma, state_state)")] - fn py_new( - tau: Option, - sigma: Option, - steady_state: Option, - bias: Option, - epoch: Option, - ) -> Result { - if tau.is_none() && sigma.is_none() && steady_state.is_none() { - // We're called from pickle, return a non initialized state - return Ok(Self::ZERO); - } else if tau.is_none() || sigma.is_none() || steady_state.is_none() { - return Err(ConfigError::InvalidConfig { - msg: "tau, sigma, and steady_state must be specified".to_string(), - }); - } - - let tau = tau.unwrap(); - let sigma = sigma.unwrap(); - let steady_state = steady_state.unwrap(); - - if tau <= Duration::ZERO { - return Err(ConfigError::InvalidConfig { - msg: format!("tau must be positive but got {tau}"), - }); - } - - Ok(Self { - tau, - bias_sigma: sigma, - steady_state_sigma: steady_state, - bias, - epoch, - }) - } - - #[cfg(feature = "python")] - #[getter] - fn get_tau(&self) -> Duration { - self.tau - } - - #[cfg(feature = "python")] - #[setter] - fn set_tau(&mut self, tau: Duration) -> PyResult<()> { - self.tau = tau; - Ok(()) - } - - #[cfg(feature = "python")] - #[getter] - fn get_bias(&self) -> Option { - self.bias - } - - #[cfg(feature = "python")] - #[setter] - fn set_sampling(&mut self, bias: f64) -> PyResult<()> { - self.bias_sigma = bias; - Ok(()) - } - - /// Initializes a new Gauss Markov process for the provided kind of model. - /// - /// Available models are: `Range`, `Doppler`, `RangeHP`, `Doppler HP` (HP stands for high precision). - #[cfg(feature = "python")] - #[classmethod] - fn default(_cls: &PyType, kind: String) -> Result { - Self::from_default(kind) - } - - #[cfg(feature = "python")] - #[classmethod] - fn load(_cls: &PyType, path: &str) -> Result { - ::load(path) - } - - #[cfg(feature = "python")] - #[classmethod] - fn load_many(_cls: &PyType, path: &str) -> Result, ConfigError> { - ::load_many(path) - } - - #[cfg(feature = "python")] - #[classmethod] - fn load_named(_cls: &PyType, path: &str) -> Result, ConfigError> { - ::load_named(path) - } - - /// Create a new `GaussMarkov` process as if it were purely a white noise, i.c. without any time correlation. - #[cfg(feature = "python")] - #[classmethod] - fn white(_cls: &PyType, sigma: f64) -> Result { - Ok(Self::white_noise(sigma)) - } - - #[cfg(feature = "python")] - /// Loads the SpacecraftDynamics from its YAML representation - #[classmethod] - fn loads(_cls: &PyType, data: &PyAny) -> Result, ConfigError> { - use snafu::ResultExt; - - use crate::io::ParseSnafu; - - if let Ok(as_list) = data.downcast::() { - let mut selves = Vec::new(); - for item in as_list.iter() { - // Check that the item is a dictionary - let next: Self = - serde_yml::from_value(pyany_to_value(item)?).context(ParseSnafu)?; - selves.push(next); - } - Ok(selves) - } else if let Ok(as_dict) = data.downcast::() { - let mut selves = Vec::new(); - for item_as_list in as_dict.items() { - let v_any = item_as_list - .get_item(1) - .map_err(|_| ConfigError::InvalidConfig { - msg: "could not get key from provided dictionary item".to_string(), - })?; - - // Try to convert the underlying data - match pyany_to_value(v_any) { - Ok(value) => { - match serde_yml::from_value(value) { - Ok(next) => selves.push(next), - Err(_) => { - // Maybe this was to be parsed in full as a single item - let me: Self = depythonize(data).map_err(|e| { - ConfigError::InvalidConfig { msg: e.to_string() } - })?; - selves.clear(); - selves.push(me); - return Ok(selves); - } - } - } - Err(_) => { - // Maybe this was to be parsed in full as a single item - let me: Self = depythonize(data) - .map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() })?; - selves.clear(); - selves.push(me); - return Ok(selves); - } - } - } - Ok(selves) - } else { - depythonize(data).map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() }) - } - } - - #[cfg(feature = "python")] - fn dumps(&self, py: Python) -> Result { - pythonize(py, &self).map_err(|e| NyxError::CustomError { msg: e.to_string() }) - } - - #[cfg(feature = "python")] - fn __getstate__(&self, py: Python) -> Result { - self.dumps(py) - } - - #[cfg(feature = "python")] - fn __setstate__(&mut self, state: &PyAny) -> Result<(), ConfigError> { - *self = - depythonize(state).map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() })?; - Ok(()) - } -} - impl ConfigRepr for GaussMarkov {} #[cfg(test)] diff --git a/src/od/process/conf.rs b/src/od/process/conf.rs index fc30bd9f..73c8e9c7 100644 --- a/src/od/process/conf.rs +++ b/src/od/process/conf.rs @@ -23,9 +23,6 @@ use std::default::Default; use std::fmt; use typed_builder::TypedBuilder; -#[cfg(feature = "python")] -use pyo3::prelude::*; - /// Defines the stopping condition for the smoother #[derive(Clone, Copy, Debug)] pub enum SmoothingArc { @@ -59,7 +56,7 @@ impl Default for SmoothingArc { /// Defines a filter iteration configuration. Allows iterating on an OD solution until convergence criteria is met. /// The root mean squared of the prefit residuals ratios is used to assess convergence between iterations. #[derive(Clone, Copy, Debug, TypedBuilder)] -#[cfg_attr(feature = "python", pyclass)] + pub struct IterationConf { /// The number of measurements to account for in the iteration #[builder(default)] @@ -91,75 +88,6 @@ impl IterationConf { } } -#[cfg(feature = "python")] -#[pymethods] -impl IterationConf { - #[new] - fn py_new( - strategy: Option, - duration: Option, - epoch: Option, - absolute_tol: Option, - relative_tol: Option, - max_iterations: Option, - max_divergences: Option, - force_failure: Option, - ) -> Result { - let mut me = Self { - smoother: if let Some(strategy) = strategy { - match strategy.to_lowercase().trim() { - "all" => SmoothingArc::All, - "prediction" => SmoothingArc::Prediction, - _ => { - return Err(NyxError::CustomError { - msg: "strategy should be `all` or `prediction`".to_string(), - }) - } - } - } else if let Some(duration) = duration { - SmoothingArc::TimeGap(duration) - } else if let Some(epoch) = epoch { - SmoothingArc::Epoch(epoch) - } else { - return Err(NyxError::CustomError { - msg: "Smoothing arc strategy not specified".to_string(), - }); - }, - ..Default::default() - }; - - if let Some(abs_tol) = absolute_tol { - me.absolute_tol = abs_tol; - } - - if let Some(rel_tol) = relative_tol { - me.relative_tol = rel_tol; - } - - if let Some(max_it) = max_iterations { - me.max_iterations = max_it; - } - - if let Some(max_div) = max_divergences { - me.max_divergences = max_div; - } - - if let Some(force_failure) = force_failure { - me.force_failure = force_failure; - } - - Ok(me) - } - - fn __repr__(&self) -> String { - format!("{self}") - } - - fn __str__(&self) -> String { - format!("Smoothing {self}") - } -} - impl Default for IterationConf { /// The default absolute tolerance is 1e-2 (calibrated on an EKF with error). fn default() -> Self { diff --git a/src/od/process/mod.rs b/src/od/process/mod.rs index ccc8732e..10e4b609 100644 --- a/src/od/process/mod.rs +++ b/src/od/process/mod.rs @@ -444,7 +444,6 @@ where self.prop.set_step(max_step, false); } - // let prop_time = measurements[num_msrs - 1].1.epoch() - self.kf.previous_estimate().epoch(); let prop_time = arc.end_epoch().unwrap() - self.kf.previous_estimate().epoch(); info!("Navigation propagating for a total of {prop_time} with step size {max_step}"); diff --git a/src/od/process/rejectcrit.rs b/src/od/process/rejectcrit.rs index acd4cf58..6062d2bb 100644 --- a/src/od/process/rejectcrit.rs +++ b/src/od/process/rejectcrit.rs @@ -17,16 +17,6 @@ */ use crate::io::ConfigRepr; -#[cfg(feature = "python")] -use crate::python::PythonError; -#[cfg(feature = "python")] -use crate::NyxError; -#[cfg(feature = "python")] -use pyo3::class::basic::CompareOp; -#[cfg(feature = "python")] -use pyo3::prelude::*; -#[cfg(feature = "python")] -use pythonize::{depythonize, pythonize}; use serde_derive::{Deserialize, Serialize}; /// Reject measurements if the prefit is greater than the provided sigmas deviation from the measurement noise. @@ -36,82 +26,11 @@ use serde_derive::{Deserialize, Serialize}; /// As such, if the prefit on range is bad, then the Doppler measurement with the same time stamp will also be rejected. /// This leads to better convergence of the filter, and more appropriate results. #[derive(Copy, Clone, Debug, Serialize, Deserialize, PartialEq)] -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.orbit_determination"))] pub struct ResidRejectCrit { /// Number of sigmas for a measurement to be considered an outlier. pub num_sigmas: f64, } -#[cfg(feature = "python")] -#[pymethods] -impl ResidRejectCrit { - #[new] - #[pyo3(text_signature = "(min_accepted=None, num_sigmas=None)")] - fn py_new(min_accepted: Option, num_sigmas: Option) -> Self { - let mut me = Self::default(); - if let Some(min_accepted) = min_accepted { - me.min_accepted = min_accepted; - } - if let Some(num_sigmas) = num_sigmas { - me.num_sigmas = num_sigmas; - } - me - } - - #[getter] - fn get_min_accepted(&self) -> usize { - self.min_accepted - } - - #[setter(orbit)] - fn py_set_min_accepted(&mut self, min_accepted: usize) -> PyResult<()> { - self.min_accepted = min_accepted; - Ok(()) - } - - #[getter] - fn get_num_sigmas(&self) -> f64 { - self.num_sigmas - } - - #[setter(orbit)] - fn py_set_num_sigmas(&mut self, num_sigmas: f64) -> PyResult<()> { - self.num_sigmas = num_sigmas; - Ok(()) - } - - fn __repr__(&self) -> String { - format!("{self:?}") - } - - fn __str__(&self) -> String { - format!("{self:?}") - } - - fn dumps(&self, py: Python) -> Result { - pythonize(py, &self).map_err(|e| NyxError::CustomError { msg: e.to_string() }) - } - - fn __getstate__(&self, py: Python) -> Result { - self.dumps(py) - } - - fn __setstate__(&mut self, state: &PyAny) -> Result<(), ConfigError> { - *self = - depythonize(state).map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() })?; - Ok(()) - } - - fn __richcmp__(&self, other: &Self, op: CompareOp) -> Result { - match op { - CompareOp::Eq => Ok(self == other), - CompareOp::Ne => Ok(self != other), - _ => Err(PythonError::OperationError { op }), - } - } -} - impl Default for ResidRejectCrit { /// By default, a measurement is rejected if its prefit residual is greater the 3-sigma value of the measurement noise at that time step. /// This corresponds to [1 chance in in 370](https://en.wikipedia.org/wiki/68%E2%80%9395%E2%80%9399.7_rule). diff --git a/src/od/simulator/scheduler.rs b/src/od/simulator/scheduler.rs index 5444269c..e1977f15 100644 --- a/src/od/simulator/scheduler.rs +++ b/src/od/simulator/scheduler.rs @@ -26,13 +26,8 @@ use serde_derive::Serialize; use std::fmt::Debug; use typed_builder::TypedBuilder; -#[cfg(feature = "python")] -use pyo3::prelude::*; - /// Defines the handoff from a current ground station to the next one that is visible to prevent overlapping of measurements #[derive(Copy, Clone, Debug, Deserialize, PartialEq, Serialize)] -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.orbit_determination"))] pub enum Handoff { /// If a new station is in visibility of the spacecraft, the "Eager" station will immediately stop tracking and switch over (default) Eager, @@ -51,8 +46,6 @@ impl Default for Handoff { /// A scheduler allows building a scheduling of spaceraft tracking for a set of ground stations. #[derive(Copy, Clone, Debug, Default, Deserialize, PartialEq, Serialize, TypedBuilder)] #[builder(doc)] -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.orbit_determination"))] pub struct Scheduler { /// Handoff strategy if two trackers see the vehicle at the same time #[builder(default)] diff --git a/src/od/simulator/trkconfig.rs b/src/od/simulator/trkconfig.rs index 7ee43ed5..0434d6e2 100644 --- a/src/od/simulator/trkconfig.rs +++ b/src/od/simulator/trkconfig.rs @@ -21,8 +21,7 @@ use crate::io::ConfigRepr; use crate::io::{duration_from_str, duration_to_str, epoch_from_str, epoch_to_str, ConfigError}; use hifitime::TimeUnits; use hifitime::{Duration, Epoch}; -#[cfg(feature = "python")] -use pyo3::prelude::*; + use serde::Deserialize; use serde_derive::Serialize; use std::fmt::Debug; @@ -33,9 +32,6 @@ use typed_builder::TypedBuilder; /// By default, the tracking configuration is continuous and the tracking arc is from the beginning of the simulation to the end. /// In Python, any value that is set to None at initialization will use the default values. #[derive(Clone, Debug, Serialize, Deserialize, PartialEq, TypedBuilder)] -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.orbit_determination"))] -#[cfg_attr(feature = "python", pyo3(get_all, set_all))] #[builder(doc)] pub struct TrkConfig { /// Set to automatically build a tracking schedule based on some criteria @@ -130,9 +126,6 @@ impl Default for TrkConfig { /// Stores a tracking strand with a start and end epoch #[derive(Copy, Clone, Debug, Serialize, Deserialize, PartialEq)] -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.orbit_determination"))] -#[cfg_attr(feature = "python", pyo3(get_all, set_all))] pub struct Strand { #[serde(serialize_with = "epoch_to_str", deserialize_with = "epoch_from_str")] pub start: Epoch, @@ -140,7 +133,6 @@ pub struct Strand { pub end: Epoch, } -#[cfg_attr(feature = "python", pymethods)] impl Strand { /// Returns whether the provided epoch is within the range pub fn contains(&self, epoch: Epoch) -> bool { @@ -151,13 +143,6 @@ impl Strand { pub fn duration(&self) -> Duration { self.end - self.start } - - /// Builds a new strand with the start and end epochs of this tracking strand. - #[cfg(feature = "python")] - #[new] - fn py_new(start: Epoch, end: Epoch) -> Self { - Self { start, end } - } } #[cfg(test)] diff --git a/src/polyfit/polynomial.rs b/src/polyfit/polynomial.rs index 8df003ba..bad9a3b8 100644 --- a/src/polyfit/polynomial.rs +++ b/src/polyfit/polynomial.rs @@ -17,7 +17,7 @@ */ /* NOTE: This code is effectively a clone of bacon-sci, MIT License, by Wyatt Campbell. */ - +use serde_derive::{Deserialize, Serialize}; use std::fmt; use std::ops; @@ -243,7 +243,7 @@ impl ops::Sub> for Polynomial f(x) = ax + b (order is FLIPPED from Polynomial structure) @@ -374,126 +374,140 @@ impl fmt::Display for CommonPolynomial { } } -#[test] -fn poly_constant() { - let c = CommonPolynomial::Constant(10.0); - for i in -100..=100 { - assert!( - (c.eval(i as f64) - 10.0).abs() < 2e-16, - "Constant polynomial returned wrong value" - ); +#[cfg(test)] +mod ut_poly { + use crate::polyfit::{CommonPolynomial, Polynomial}; + + #[test] + fn poly_constant() { + let c = CommonPolynomial::Constant(10.0); + for i in -100..=100 { + assert!( + (c.eval(i as f64) - 10.0).abs() < f64::EPSILON, + "Constant polynomial returned wrong value" + ); + } } -} -#[test] -fn poly_linear() { - let c = CommonPolynomial::Linear(2.0, 10.0); - for i in -100..=100 { - let x = i as f64; - let expect = 2.0 * x + 10.0; - assert!( - (c.eval(x) - expect).abs() < 2e-16, - "Constant polynomial returned wrong value" - ); + #[test] + fn poly_linear() { + let c = CommonPolynomial::Linear(2.0, 10.0); + for i in -100..=100 { + let x = i as f64; + let expect = 2.0 * x + 10.0; + assert!( + (c.eval(x) - expect).abs() < f64::EPSILON, + "Constant polynomial returned wrong value" + ); + } } -} -#[test] -fn poly_quadratic() { - let p = Polynomial { - coefficients: [101.0, -2.0, 3.0], - }; - let p2 = 2.0 * p; - let c = CommonPolynomial::Quadratic(3.0, -2.0, 101.0); - for i in -100..=100 { - let x = i as f64; - let expect = 3.0 * x.powi(2) - 2.0 * x + 101.0; - let expect_deriv = 6.0 * x - 2.0; - assert!( - (c.eval(x) - expect).abs() < 2e-16, - "Polynomial returned wrong value" - ); - assert!( - (p.deriv(x) - expect_deriv).abs() < 2e-16, - "Polynomial derivative returned wrong value" - ); + #[test] + fn poly_quadratic() { + let p = Polynomial { + coefficients: [101.0, -2.0, 3.0], + }; + let p2 = 2.0 * p; + let c = CommonPolynomial::Quadratic(3.0, -2.0, 101.0); + for i in -100..=100 { + let x = i as f64; + let expect = 3.0 * x.powi(2) - 2.0 * x + 101.0; + let expect_deriv = 6.0 * x - 2.0; + assert!( + (c.eval(x) - expect).abs() < f64::EPSILON, + "Polynomial returned wrong value" + ); + assert!( + (p.deriv(x) - expect_deriv).abs() < f64::EPSILON, + "Polynomial derivative returned wrong value" + ); + + assert!( + (p.eval(x) - expect).abs() < f64::EPSILON, + "Polynomial returned wrong value" + ); + assert!( + (p2.eval(x) - 2.0 * expect).abs() < f64::EPSILON, + "Polynomial returned wrong value" + ); + } + } - assert!( - (p.eval(x) - expect).abs() < 2e-16, - "Polynomial returned wrong value" - ); - assert!( - (p2.eval(x) - 2.0 * expect).abs() < 2e-16, - "Polynomial returned wrong value" + #[test] + fn poly_print() { + let p = Polynomial { + coefficients: [101.0, -2.0, 3.0], + }; + println!("{}", p); + assert_eq!( + format!("{}", p), + format!("{}", CommonPolynomial::Quadratic(3.0, -2.0, 101.0)) ); } -} -#[test] -fn poly_print() { - let p = Polynomial { - coefficients: [101.0, -2.0, 3.0], - }; - println!("{}", p); - assert_eq!( - format!("{}", p), - format!("{}", CommonPolynomial::Quadratic(3.0, -2.0, 101.0)) - ); -} + #[test] + fn poly_add() { + let p1 = Polynomial { + coefficients: [4.0, -2.0, 3.0], + }; + let p2 = Polynomial { + coefficients: [0.0, -5.0, 0.0, 2.0], + }; + // P(x) = (3x^2 - 2x + 4) + (2x^3 - 5x) + // <=> P(x) = 2x^3 + 3x^2 -7x + 4 + let p_expected = Polynomial { + coefficients: [4.0, -7.0, 3.0, 2.0], + }; -#[test] -fn poly_add() { - let p1 = Polynomial { - coefficients: [4.0, -2.0, 3.0], - }; - let p2 = Polynomial { - coefficients: [0.0, -5.0, 0.0, 2.0], - }; - // P(x) = (3x^2 - 2x + 4) + (2x^3 - 5x) - // <=> P(x) = 2x^3 + 3x^2 -7x + 4 - let p_expected = Polynomial { - coefficients: [4.0, -7.0, 3.0, 2.0], - }; - - // let p3 = add::<4, 3>(p2, p1); - let p3 = p1 + p2; - println!("p3 = {:x}\npe = {:x}", p3, p_expected); - assert_eq!(p3, p_expected); - // Check this is correct - for i in -100..=100 { - let x = i as f64; - let expect = p1.eval(x) + p2.eval(x); - assert!( - (p3.eval(x) - expect).abs() < 2e-16, - "Constant polynomial returned wrong value" - ); + // let p3 = add::<4, 3>(p2, p1); + let p3 = p1 + p2; + println!("p3 = {:x}\npe = {:x}", p3, p_expected); + assert_eq!(p3, p_expected); + // Check this is correct + for i in -100..=100 { + let x = i as f64; + let expect = p1.eval(x) + p2.eval(x); + assert!( + (p3.eval(x) - expect).abs() < f64::EPSILON, + "Constant polynomial returned wrong value" + ); + } } -} -#[test] -fn poly_sub() { - let p2 = Polynomial { - coefficients: [4.0, -2.0, 3.0], - }; - let p1 = Polynomial { - coefficients: [0.0, -5.0, 0.0, 2.0], - }; - // P(x) = (3x^2 - 2x + 4) + (2x^3 - 5x) - // <=> P(x) = 2x^3 + 3x^2 -7x + 4 - let p_expected = Polynomial { - coefficients: [-4.0, -3.0, -3.0, 2.0], - }; - - let p3 = p1 - p2; - println!("p3 = {:x}\npe = {:x}", p3, p_expected); - assert_eq!(p3, p_expected); - // Check this is correct - for i in -100..=100 { - let x = i as f64; - let expect = p1.eval(x) - p2.eval(x); - assert!( - (p3.eval(x) - expect).abs() < 2e-16, - "Constant polynomial returned wrong value" - ); + #[test] + fn poly_sub() { + let p2 = Polynomial { + coefficients: [4.0, -2.0, 3.0], + }; + let p1 = Polynomial { + coefficients: [0.0, -5.0, 0.0, 2.0], + }; + // P(x) = (3x^2 - 2x + 4) + (2x^3 - 5x) + // <=> P(x) = 2x^3 + 3x^2 -7x + 4 + let p_expected = Polynomial { + coefficients: [-4.0, -3.0, -3.0, 2.0], + }; + + let p3 = p1 - p2; + println!("p3 = {:x}\npe = {:x}", p3, p_expected); + assert_eq!(p3, p_expected); + // Check this is correct + for i in -100..=100 { + let x = i as f64; + let expect = p1.eval(x) - p2.eval(x); + assert!( + (p3.eval(x) - expect).abs() < f64::EPSILON, + "Constant polynomial returned wrong value" + ); + } + } + + #[test] + fn poly_serde() { + let c = CommonPolynomial::Quadratic(3.0, -2.0, 101.0); + let c_yml = serde_yml::to_string(&c).unwrap(); + println!("{c_yml}"); + let c2 = serde_yml::from_str(&c_yml).unwrap(); + assert_eq!(c, c2); } } diff --git a/src/python/cosmic.rs b/src/python/cosmic.rs deleted file mode 100644 index 4ac1a383..00000000 --- a/src/python/cosmic.rs +++ /dev/null @@ -1,115 +0,0 @@ -use pyo3::prelude::*; -use pyo3::py_run; -use pyo3::types::PyType; - -pub use crate::cosmic::Bodies; -use crate::cosmic::Cosm as CosmRs; -use crate::cosmic::Frame as FrameRs; -use crate::cosmic::GuidanceMode; -pub use crate::cosmic::Orbit; -pub use crate::cosmic::{DragConfig, Spacecraft, SrpConfig}; -use crate::dynamics::guidance::Thruster; -use std::sync::Arc; - -pub(crate) fn register_cosmic(py: Python<'_>, parent_module: &PyModule) -> PyResult<()> { - let sm = PyModule::new(py, "_nyx_space.cosmic")?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - - py_run!(py, sm, "import sys; sys.modules['nyx_space.cosmic'] = sm"); - parent_module.add_submodule(sm)?; - Ok(()) -} - -#[derive(Clone)] -#[pyclass] -pub struct Frame { - pub inner: FrameRs, -} - -#[pymethods] -impl Frame { - pub fn is_geoid(&self) -> bool { - self.inner.is_geoid() - } - pub fn is_celestial(&self) -> bool { - self.inner.is_celestial() - } - pub fn ephem_path(&self) -> Vec { - self.inner.ephem_path() - } - pub fn frame_path(&self) -> Vec { - self.inner.frame_path() - } - pub fn gm(&self) -> f64 { - self.inner.gm() - } - pub fn equatorial_radius(&self) -> f64 { - self.inner.equatorial_radius() - } - pub fn flattening(&self) -> f64 { - self.inner.flattening() - } - pub fn semi_major_radius(&self) -> f64 { - self.inner.semi_major_radius() - } - pub fn angular_velocity(&self) -> f64 { - self.inner.angular_velocity() - } -} - -#[pyclass] -pub struct Cosm { - pub inner: Arc, -} - -#[pymethods] -impl Cosm { - #[classmethod] - pub fn from_xb(_cls: &PyType, filename: &str) -> PyResult { - Ok(Cosm { - inner: Arc::new(CosmRs::from_xb(filename)?), - }) - } - #[classmethod] - pub fn try_de438(_cls: &PyType) -> PyResult { - Ok(Cosm { - inner: Arc::new(CosmRs::try_de438()?), - }) - } - #[classmethod] - pub fn de438(_cls: &PyType) -> PyResult { - Ok(Cosm { - inner: CosmRs::de438(), - }) - } - #[classmethod] - pub fn de438_raw(_cls: &PyType) -> PyResult { - Ok(Cosm { - inner: Arc::new(CosmRs::de438_raw()), - }) - } - #[classmethod] - pub fn de438_gmat(_cls: &PyType) -> PyResult { - Ok(Cosm { - inner: CosmRs::de438_gmat(), - }) - } - - pub fn frame(&self, name: &str) -> PyResult { - Ok(Frame { - inner: self.inner.try_frame(name)?, - }) - } - - pub fn frames_get_names(&self) -> PyResult> { - Ok(self.inner.frames_get_names()) - } -} diff --git a/src/python/mission_design/events.rs b/src/python/mission_design/events.rs deleted file mode 100644 index c1ae348d..00000000 --- a/src/python/mission_design/events.rs +++ /dev/null @@ -1,59 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use pyo3::prelude::*; - -use crate::md::{Event, StateParameter}; -use hifitime::Unit; - -#[pymethods] -impl Event { - /// Initializes a new event. Arguments are "parameter: StateParameter" and "desired_value: float". - #[new] - #[pyo3( - text_signature = "(parameter, desired_value, epoch_precision=None, value_precision=None)" - )] - fn py_new( - parameter: StateParameter, - desired_value: f64, - epoch_precision: Option, - value_precision: Option, - ) -> Self { - if let Some(value_precision) = value_precision { - if let Some(epoch_precision) = epoch_precision { - Self::specific(parameter, desired_value, value_precision, epoch_precision) - } else { - Self::within_tolerance(parameter, desired_value, value_precision) - } - } else if let Some(epoch_precision) = epoch_precision { - Self::specific( - parameter, - desired_value, - parameter.default_event_precision(), - epoch_precision, - ) - } else { - Self::new(parameter, desired_value) - } - } - - #[cfg(feature = "python")] - fn __str__(&self) -> String { - format!("{self}") - } -} diff --git a/src/python/mission_design/mod.rs b/src/python/mission_design/mod.rs deleted file mode 100644 index d454b5d0..00000000 --- a/src/python/mission_design/mod.rs +++ /dev/null @@ -1,238 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use crate::io::trajectory_data::TrajectoryLoader; -use crate::io::{ConfigError, ExportCfg}; -use crate::md::prelude::{PropOpts, Propagator, SpacecraftDynamics}; -use crate::md::{Event, StateParameter}; -use crate::propagators::{ - CashKarp45, Dormand45, Dormand78, Fehlberg45, PropagationError, RK2Fixed, RK4Fixed, Verner56, -}; -use crate::{NyxError, Orbit, Spacecraft}; -use hifitime::{Duration, Epoch, Unit}; -use pyo3::{prelude::*, py_run}; -use rayon::prelude::*; - -pub(crate) use self::orbit_trajectory::OrbitTraj; -pub(crate) use self::sc_trajectory::SpacecraftTraj; - -mod events; -mod orbit_trajectory; -mod sc_trajectory; -pub mod spacecraft; - -/// Mission design -pub(crate) fn register_md(py: Python<'_>, parent_module: &PyModule) -> PyResult<()> { - let sm = PyModule::new(py, "_nyx_space.mission_design")?; - - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_function(wrap_pyfunction!(propagate, sm)?)?; - sm.add_function(wrap_pyfunction!(two_body, sm)?)?; - - py_run!( - py, - sm, - "import sys; sys.modules['nyx_space.mission_design'] = sm" - ); - parent_module.add_submodule(sm)?; - Ok(()) -} - -/// Propagates the provided spacecraft with the provided dynamics until the provided stopping condition (duration, epoch, or event [and optionally the count]). -/// -/// Available methods: rk89, dormand78, dormand45, rk45 (or fehlberg45), cashkarp45, verner56, rk4, rk2 -#[pyfunction] -#[pyo3( - text_signature = "(spacecraft, dynamics, duration=None, epoch=None, event=None, event_count=None, min_step=None, max_step=None, fixed_step=None, tolerance=None, method='rk89')" -)] -fn propagate( - spacecraft: Spacecraft, - dynamics: SpacecraftDynamics, - duration: Option, - epoch: Option, - event: Option, - event_count: Option, - min_step: Option, - max_step: Option, - fixed_step: Option, - tolerance: Option, - method: Option, -) -> Result<(Spacecraft, SpacecraftTraj), PropagationError> { - let opts = match fixed_step { - Some(step) => PropOpts::with_fixed_step(step), - None => { - let mut opts = PropOpts::default(); - if let Some(step) = min_step { - opts.set_min_step(step); - } - if let Some(step) = max_step { - opts.set_max_step(step); - } - if let Some(tol) = tolerance { - opts.tolerance = tol; - } - opts - } - }; - info!("Propagator options: {opts}"); - - let prop_setup = match method { - Some(value) => match value.to_lowercase().as_str() { - "rk89" => Propagator::rk89(dynamics, opts), - "dormand78" => Propagator::new::(dynamics, opts), - "dormand45" => Propagator::new::(dynamics, opts), - "rk45" | "fehlberg45" => Propagator::new::(dynamics, opts), - "cashkarp45" => Propagator::new::(dynamics, opts), - "verner56" => Propagator::new::(dynamics, opts), - "rk4" => Propagator::new::(dynamics, opts), - "rk2" => Propagator::new::(dynamics, opts), - _ => { - return Err(PropagationError::PropConfigError { - source: ConfigError::InvalidConfig { - msg: format!("Unknown propagation method: {}", value), - }, - }) - } - }, - None => Propagator::rk89(dynamics, opts), - }; - - if let Some(event) = event { - let max_duration = match duration { - Some(duration) => duration, - None => { - warn!("No maximum duration provided to search, setting to 30 days"); - 30 * Unit::Day - } - }; - - let (sc, traj) = match event_count { - Some(count) => { - prop_setup - .with(spacecraft) - .until_nth_event(max_duration, &event, count)? - } - None => prop_setup - .with(spacecraft) - .until_event(max_duration, &event)?, - }; - - Ok((sc, SpacecraftTraj { inner: traj })) - } else if let Some(duration) = duration { - let (sc, traj) = prop_setup - .with(spacecraft) - .for_duration_with_traj(duration)?; - - Ok((sc, SpacecraftTraj { inner: traj })) - } else if let Some(epoch) = epoch { - let (sc, traj) = prop_setup.with(spacecraft).until_epoch_with_traj(epoch)?; - - Ok((sc, SpacecraftTraj { inner: traj })) - } else { - Err(PropagationError::PropConfigError { - source: ConfigError::InvalidConfig { - msg: "Either duration or epoch must be provided for a propagation to happen" - .to_string(), - }, - }) - } -} - -/// Performs a two body propagation around the central body of each orbit in the `orbits` list either for the duration in the list or until the epochs in the list. -/// New epoch and duration parameters may either be exactly one item or N items, where N is the number of orbits. -/// This computation will happen in parallel on all CPUs. It uses the `at_epoch` function of `Orbit`. -#[pyfunction] -fn two_body( - orbits: Vec, - new_epochs: Option>, - durations: Option>, -) -> Result, NyxError> { - let rslt_epochs: Result, NyxError> = if (new_epochs.is_some() && durations.is_some()) - || (new_epochs.is_none() && durations.is_none()) - { - Err(NyxError::ConfigError { - source: ConfigError::InvalidConfig { - msg: "Either duration or epoch must be provided for a propagation".to_string(), - }, - }) - } else if let Some(new_epochs) = new_epochs { - if new_epochs.len() == 1 { - Ok(vec![new_epochs[0]; orbits.len()]) - } else if new_epochs.len() == orbits.len() { - Ok(new_epochs) - } else { - Err(NyxError::ConfigError { - source: ConfigError::InvalidConfig { - msg: format!( - "Expecting either one or {} items in epochs vector", - orbits.len() - ), - }, - }) - } - } else if let Some(durations) = durations { - if durations.len() == 1 { - Ok(orbits - .iter() - .map(|orbit| orbit.epoch + durations[0]) - .collect()) - } else if durations.len() == orbits.len() { - Ok(durations - .iter() - .zip(&orbits) - .map(|(duration, orbit)| orbit.epoch + *duration) - .collect()) - } else { - Err(NyxError::ConfigError { - source: ConfigError::InvalidConfig { - msg: format!( - "Expecting either one or {} items in durations vector", - orbits.len() - ), - }, - }) - } - } else { - Err(NyxError::CustomError { - msg: "you have entered unreachable code, please report a bug".to_string(), - }) - }; - - let epochs = rslt_epochs?; - - Ok((orbits, epochs) - .into_par_iter() - .map(|(orbit, epoch)| orbit.at_epoch(epoch)) - .collect::>>() - .into_iter() - .filter(|result| match result { - Ok(_) => true, - Err(e) => { - println!("Error: {e:?}"); - false - } - }) - .map(|s| s.unwrap()) - .collect::>()) -} diff --git a/src/python/mission_design/orbit_trajectory.rs b/src/python/mission_design/orbit_trajectory.rs deleted file mode 100644 index e8e7da80..00000000 --- a/src/python/mission_design/orbit_trajectory.rs +++ /dev/null @@ -1,220 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use hifitime::{Duration, Epoch, Unit}; -use pyo3::prelude::*; - -use super::SpacecraftTraj; -use crate::md::trajectory::{ExportCfg, TrajError}; -use crate::python::cosmic::Frame; -use crate::{ - md::{ - prelude::{Cosm, Traj as TrajRs}, - Event, EventEvaluator, - }, - NyxError, Orbit, Spacecraft, State, -}; - -use std::collections::HashMap; - -/// An orbit trajectory. Cannot be pickled in Python, so you must export it to Parquet first and use the TrajectoryLoader. -#[pyclass] -pub(crate) struct OrbitTraj { - pub(crate) inner: TrajRs, -} - -#[pymethods] -impl OrbitTraj { - /// Convert this orbit trajectory into a spacecraft trajectory by copying the provided template and setting its orbit state to that of each state of the trajectory - pub fn upcast(&self, template: Spacecraft) -> SpacecraftTraj { - SpacecraftTraj { - inner: self.inner.upcast(template), - } - } - - /// Returns the state at the provided epoch, or raises an exception if the epoch is outside of the bounds of the trajectory - fn at(&self, epoch: Epoch) -> Result { - self.inner.at(epoch) - } - - /// Return the first state of the trajectory - fn first(&self) -> Orbit { - *self.inner.first() - } - - /// Return the last state of the trajectory - fn last(&self) -> Orbit { - *self.inner.last() - } - - /// Copies this object and resamples it with the provided step size - fn resample(&self, step: Duration) -> Result { - let inner = self.inner.resample(step)?; - - Ok(Self { inner }) - } - - /// Copies this object and rebuilds it with the provided epochs - fn rebuild(&self, epochs: Vec) -> Result { - let inner = self.inner.rebuild(&epochs)?; - - Ok(Self { inner }) - } - - /// Finds a specific event in a trajectory. - /// - /// If a start or end epoch is provided (or both are provided), this function will return a list of a single event. - /// If none are provided, this function will search the whole trajectory for the event and return all of the states where such event happens. - fn find( - &self, - event: Event, - start: Option, - end: Option, - ) -> Result, NyxError> { - if start.is_some() || end.is_some() { - let start = if let Some(start) = start { - start - } else { - self.inner.first().epoch() - }; - - let end = if let Some(end) = end { - end - } else { - self.inner.last().epoch() - }; - - Ok(vec![self.inner.find_bracketed(start, end, &event)?.state]) - } else { - Ok(self - .inner - .find(&event)? - .iter() - .map(|details| details.state) - .collect::>()) - } - } - - /// Find the minimum and maximum of the provided event through the trajectory with a specified time unit precision. - pub fn find_minmax(&self, event: Event, precision: Unit) -> Result<(Orbit, Orbit), NyxError> { - self.inner.find_minmax(&event, precision) - } - - /// Saves this trajectory to a parquet file, optionally adding the event columns to append and metadata. - /// Set the groundtrack parameter to a body fixed frame to export this trajectory with latitude, longitude, and height columns in that body fixed frame. - #[pyo3(text_signature = "(path, events=None, metadata=None, groundtrack=None")] - fn to_parquet( - &self, - path: String, - events: Option>, - metadata: Option>, - groundtrack: Option, - ) -> Result { - let maybe = match events { - None => match groundtrack { - None => { - let cfg = match metadata { - None => ExportCfg::default(), - Some(_) => ExportCfg { - metadata, - ..Default::default() - }, - }; - self.inner.to_parquet(path, None, cfg) - } - Some(py_frame) => self.inner.to_groundtrack_parquet( - path, - py_frame.inner, - None, - metadata, - Cosm::de438(), - ), - }, - Some(events) => { - let events = events - .iter() - .map(|e| e as &dyn EventEvaluator) - .collect::>>(); - - match groundtrack { - None => { - let cfg = match metadata { - None => ExportCfg::default(), - Some(_) => ExportCfg { - metadata, - ..Default::default() - }, - }; - self.inner.to_parquet(path, Some(events), cfg) - } - Some(py_frame) => self.inner.to_groundtrack_parquet( - path, - py_frame.inner, - Some(events), - metadata, - Cosm::de438(), - ), - } - } - }; - - match maybe { - Ok(path) => Ok(format!("{}", path.to_str().unwrap())), - Err(e) => Err(NyxError::CustomError { msg: e.to_string() }), - } - } - - /// Allows converting the source trajectory into the (almost) equivalent trajectory in another frame. - /// This simply converts each state into the other frame and may lead to aliasing due to the Nyquist–Shannon sampling theorem. - fn to_frame(&self, new_frame: String) -> Result { - let cosm = Cosm::de438(); - - let frame = cosm.try_frame(&new_frame)?; - - let conv_traj = self.inner.to_frame(frame, cosm)?; - - Ok(Self { inner: conv_traj }) - } - - /// Compute the RIC difference between this trajectory and another, writing the output to a parquet file. - fn ric_diff_to_parquet( - &self, - other: &Self, - path: String, - cfg: Option, - ) -> Result { - match self.inner.ric_diff_to_parquet( - &other.inner, - path, - cfg.unwrap_or_else(|| ExportCfg::default()), - ) { - Ok(path) => Ok(format!("{}", path.to_str().unwrap())), - Err(e) => Err(NyxError::CustomError { msg: e.to_string() }), - } - } - - fn __add__(&self, rhs: &Self) -> Result { - let inner = (self.inner.clone() + rhs.inner.clone())?; - - Ok(Self { inner }) - } - - fn __str__(&self) -> String { - format!("{}", self.inner) - } -} diff --git a/src/python/mission_design/sc_trajectory.rs b/src/python/mission_design/sc_trajectory.rs deleted file mode 100644 index 6b67c050..00000000 --- a/src/python/mission_design/sc_trajectory.rs +++ /dev/null @@ -1,224 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use hifitime::{Duration, Epoch, Unit}; -use pyo3::prelude::*; - -use crate::md::trajectory::{ExportCfg, TrajError}; -use crate::python::cosmic::Frame; -use crate::{ - md::{ - prelude::{Cosm, Traj as TrajRs}, - Event, EventEvaluator, - }, - NyxError, Spacecraft, State, -}; - -use super::OrbitTraj; -use std::collections::HashMap; - -/// A structure that stores a spacecraft structure generated from a propagation. -/// Cannot be pickled in Python, so you must export it to Parquet first and use the TrajectoryLoader. -#[pyclass] -pub(crate) struct SpacecraftTraj { - pub(crate) inner: TrajRs, -} - -#[pymethods] -impl SpacecraftTraj { - /// Convert this spacecraft trajectory into an Orbit trajectory, loosing all references to the spacecraft - pub fn downcast(&self) -> OrbitTraj { - OrbitTraj { - inner: self.inner.downcast(), - } - } - - /// Returns the state at the provided epoch, or raises an exception if the epoch is outside of the bounds of the trajectory - fn at(&self, epoch: Epoch) -> Result { - self.inner.at(epoch) - } - - /// Return the first state of the trajectory - fn first(&self) -> Spacecraft { - *self.inner.first() - } - - /// Return the last state of the trajectory - fn last(&self) -> Spacecraft { - *self.inner.last() - } - - /// Copies this object and resamples it with the provided step size - fn resample(&self, step: Duration) -> Result { - let inner = self.inner.resample(step)?; - - Ok(Self { inner }) - } - - /// Copies this object and rebuilds it with the provided epochs - fn rebuild(&self, epochs: Vec) -> Result { - let inner = self.inner.rebuild(&epochs)?; - - Ok(Self { inner }) - } - - /// Finds a specific event in a trajectory. - /// - /// If a start or end epoch is provided (or both are provided), this function will return a list of a single event. - /// If none are provided, this function will search the whole trajectory for the event and return all of the states where such event happens. - fn find( - &self, - event: Event, - start: Option, - end: Option, - ) -> Result, NyxError> { - if start.is_some() || end.is_some() { - let start = if let Some(start) = start { - start - } else { - self.inner.first().epoch() - }; - - let end = if let Some(end) = end { - end - } else { - self.inner.last().epoch() - }; - - Ok(vec![self.inner.find_bracketed(start, end, &event)?.state]) - } else { - Ok(self - .inner - .find(&event)? - .iter() - .map(|details| details.state) - .collect::>()) - } - } - - /// Find the minimum and maximum of the provided event through the trajectory with a specified time unit precision. - pub fn find_minmax( - &self, - event: Event, - precision: Unit, - ) -> Result<(Spacecraft, Spacecraft), NyxError> { - self.inner.find_minmax(&event, precision) - } - - /// Saves this trajectory to a parquet file, optionally adding the event columns to append and metadata. - /// Set the groundtrack parameter to a body fixed frame to export this trajectory with latitude, longitude, and height columns in that body fixed frame. - #[pyo3(text_signature = "(path, events=None, metadata=None, groundtrack=None")] - fn to_parquet( - &self, - path: String, - events: Option>, - metadata: Option>, - groundtrack: Option, - ) -> Result { - let maybe = match events { - None => match groundtrack { - None => { - let cfg = match metadata { - None => ExportCfg::default(), - Some(_) => ExportCfg { - metadata, - ..Default::default() - }, - }; - self.inner.to_parquet(path, None, cfg) - } - Some(py_frame) => self.inner.to_groundtrack_parquet( - path, - py_frame.inner, - None, - metadata, - Cosm::de438(), - ), - }, - Some(events) => { - let events = events - .iter() - .map(|e| e as &dyn EventEvaluator) - .collect::>>(); - - match groundtrack { - None => { - let cfg = match metadata { - None => ExportCfg::default(), - Some(_) => ExportCfg { - metadata, - ..Default::default() - }, - }; - self.inner.to_parquet(path, Some(events), cfg) - } - Some(py_frame) => self.inner.to_groundtrack_parquet( - path, - py_frame.inner, - Some(events), - metadata, - Cosm::de438(), - ), - } - } - }; - - match maybe { - Ok(path) => Ok(format!("{}", path.to_str().unwrap())), - Err(e) => Err(NyxError::CustomError { msg: e.to_string() }), - } - } - - /// Allows converting the source trajectory into the (almost) equivalent trajectory in another frame. - /// This simply converts each state into the other frame and may lead to aliasing due to the Nyquist–Shannon sampling theorem. - fn to_frame(&self, new_frame: String) -> Result { - let cosm = Cosm::de438(); - - let frame = cosm.try_frame(&new_frame)?; - - let conv_traj = self.inner.to_frame(frame, cosm)?; - - Ok(Self { inner: conv_traj }) - } - - fn ric_diff_to_parquet( - &self, - other: &Self, - path: String, - cfg: Option, - ) -> Result { - match self.inner.ric_diff_to_parquet( - &other.inner, - path, - cfg.unwrap_or_else(|| ExportCfg::default()), - ) { - Ok(path) => Ok(format!("{}", path.to_str().unwrap())), - Err(e) => Err(NyxError::CustomError { msg: e.to_string() }), - } - } - - fn __add__(&self, rhs: &Self) -> Result { - let inner = (self.inner.clone() + rhs.inner.clone())?; - - Ok(Self { inner }) - } - - fn __str__(&self) -> String { - format!("{}", self.inner) - } -} diff --git a/src/python/mission_design/spacecraft.rs b/src/python/mission_design/spacecraft.rs deleted file mode 100644 index ee69c351..00000000 --- a/src/python/mission_design/spacecraft.rs +++ /dev/null @@ -1,272 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ -use crate::python::PythonError; -use crate::{ - cosmic::{DragConfig, SrpConfig}, - dynamics::guidance::Thruster, - io::{ConfigError, ConfigRepr}, - md::prelude::GuidanceMode, - Orbit, Spacecraft, State, -}; -use crate::{md::StateParameter, NyxError}; -use std::collections::BTreeMap; - -use hifitime::Epoch; -use pyo3::class::basic::CompareOp; -use pyo3::prelude::*; -use pyo3::types::PyType; -use pythonize::{depythonize, pythonize}; - -#[pymethods] -impl Spacecraft { - /// Initialize a new Spacecraft with optional thruster, mode, SRP, and Drag parameters. - #[new] - #[pyo3( - text_signature = "(orbit, dry_mass_kg, fuel_mass_kg, srp_area_m2, drag_area_m2, cr, cd, thruster, mode)" - )] - pub fn py_new( - orbit: Option, - dry_mass_kg: Option, - fuel_mass_kg: Option, - srp: Option, - drag: Option, - thruster: Option, - mode: Option, - ) -> Result { - if orbit.is_none() && dry_mass_kg.is_none() && fuel_mass_kg.is_none() { - Ok(Self::default()) - } else if orbit.is_none() || dry_mass_kg.is_none() || fuel_mass_kg.is_none() { - Err(NyxError::CustomError { - msg: format!("orbit and dry_mass_kg must be specified"), - }) - } else { - Ok(Self { - orbit: orbit.unwrap(), - dry_mass_kg: dry_mass_kg.unwrap(), - fuel_mass_kg: fuel_mass_kg.unwrap_or(0.0), - thruster, - mode: mode.unwrap_or(GuidanceMode::Coast), - stm: None, - srp: srp.unwrap_or_else(|| SrpConfig::default()), - drag: drag.unwrap_or_else(|| DragConfig::default()), - }) - } - } - - #[classmethod] - fn load(_cls: &PyType, path: &str) -> Result { - ::load(path) - } - - #[classmethod] - fn load_many(_cls: &PyType, path: &str) -> Result, ConfigError> { - ::load_many(path) - } - - #[classmethod] - fn load_named(_cls: &PyType, path: &str) -> Result, ConfigError> { - ::load_named(path) - } - - fn __repr__(&self) -> String { - format!("{self:?} @ {self:p}") - } - - fn __str__(&self) -> String { - format!("{self}\n{self:x}") - } - - fn __richcmp__(&self, other: &Self, op: CompareOp) -> Result { - match op { - CompareOp::Eq => Ok(self == other), - CompareOp::Ne => Ok(self != other), - _ => Err(PythonError::OperationError { op }), - } - } - - fn dumps(&self, py: Python) -> Result { - pythonize(py, &self).map_err(|e| NyxError::CustomError { msg: e.to_string() }) - } - - fn __getstate__(&self, py: Python) -> Result { - self.dumps(py) - } - - #[classmethod] - /// Loads the SpacecraftDynamics from its YAML representation - fn loads(_cls: &PyType, state: &PyAny) -> Result { - depythonize(state).map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() }) - } - - fn __setstate__(&mut self, state: &PyAny) -> Result<(), ConfigError> { - *self = - depythonize(state).map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() })?; - Ok(()) - } - - /// Note: this returns a COPY of the orbit, not a mutable reference to it! - #[getter] - fn get_orbit(&self) -> Orbit { - self.orbit - } - - #[setter(orbit)] - fn py_set_orbit(&mut self, orbit: Orbit) -> PyResult<()> { - self.orbit = orbit; - Ok(()) - } - - /// Note: this returns a COPY of the epoch, not a mutable reference to it! - #[getter] - fn get_epoch(&self) -> Epoch { - self.orbit.epoch - } - - /// Returns the value of the provided state parameter if available - fn value_of(&self, param: StateParameter) -> Result { - self.value(param) - } - - fn srp(&self) -> SrpConfig { - self.srp - } - - fn drag(&self) -> DragConfig { - self.drag - } -} - -#[pymethods] -impl SrpConfig { - #[new] - #[pyo3(text_signature = "(area_m2, cr=1.8)")] - pub fn py_new(area_m2: Option, cr: Option) -> Self { - Self { - area_m2: area_m2.unwrap_or(0.0), - cr: cr.unwrap_or(1.8), - } - } - - fn __str__(&self) -> String { - format!("{self:?}") - } - - fn __richcmp__(&self, other: &Self, op: CompareOp) -> Result { - match op { - CompareOp::Eq => Ok(self == other), - CompareOp::Ne => Ok(self != other), - _ => Err(PythonError::OperationError { op }), - } - } - - fn dumps(&self, py: Python) -> Result { - pythonize(py, &self).map_err(|e| NyxError::CustomError { msg: e.to_string() }) - } - - fn __getstate__(&self, py: Python) -> Result { - self.dumps(py) - } - - fn __setstate__(&mut self, state: &PyAny) -> Result<(), ConfigError> { - *self = - depythonize(state).map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() })?; - Ok(()) - } - - #[getter] - fn get_area_m2(&self) -> f64 { - self.area_m2 - } - - #[getter] - fn get_cr(&self) -> f64 { - self.cr - } - - #[setter] - fn set_area_m2(&mut self, area_m2: f64) -> PyResult<()> { - self.area_m2 = area_m2; - Ok(()) - } - - #[setter] - fn set_cr(&mut self, cr: f64) -> PyResult<()> { - self.cr = cr; - Ok(()) - } -} - -#[pymethods] -impl DragConfig { - #[new] - #[pyo3(text_signature = "(area_m2, cd=2.2)")] - pub fn py_new(area_m2: Option, cd: Option) -> Self { - Self { - area_m2: area_m2.unwrap_or(0.0), - cd: cd.unwrap_or(1.8), - } - } - - fn __str__(&self) -> String { - format!("{self:?}") - } - - fn __richcmp__(&self, other: &Self, op: CompareOp) -> Result { - match op { - CompareOp::Eq => Ok(self == other), - CompareOp::Ne => Ok(self != other), - _ => Err(PythonError::OperationError { op }), - } - } - - fn dumps(&self, py: Python) -> Result { - pythonize(py, &self).map_err(|e| NyxError::CustomError { msg: e.to_string() }) - } - - fn __getstate__(&self, py: Python) -> Result { - self.dumps(py) - } - - fn __setstate__(&mut self, state: &PyAny) -> Result<(), ConfigError> { - *self = - depythonize(state).map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() })?; - Ok(()) - } - - #[getter] - fn get_area_m2(&self) -> f64 { - self.area_m2 - } - - #[getter] - fn get_cd(&self) -> f64 { - self.cd - } - - #[setter] - fn set_area_m2(&mut self, area_m2: f64) -> PyResult<()> { - self.area_m2 = area_m2; - Ok(()) - } - - #[setter] - fn set_cr(&mut self, cd: f64) -> PyResult<()> { - self.cd = cd; - Ok(()) - } -} diff --git a/src/python/mod.rs b/src/python/mod.rs deleted file mode 100644 index 9ce6d91f..00000000 --- a/src/python/mod.rs +++ /dev/null @@ -1,124 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use snafu::prelude::*; - -use crate::cosmic::AstroError; -use crate::io::{ConfigError, InputOutputError}; -use crate::md::trajectory::TrajError; -use crate::od::ODError; -use crate::propagators::PropagationError; -use crate::NyxError; -use hifitime::leap_seconds::{LatestLeapSeconds, LeapSecondsFile}; -use hifitime::prelude::*; -use hifitime::ut1::Ut1Provider; -use pyo3::py_run; -use pyo3::{exceptions::PyException, prelude::*}; - -pub(crate) mod cosmic; -pub(crate) mod mission_design; -mod monte_carlo; -mod orbit_determination; -pub(crate) mod pyo3utils; - -use pyo3::class::basic::CompareOp; - -#[derive(Snafu, Debug)] -pub(crate) enum PythonError { - #[snafu(display("operation {op:?} not available on this type"))] - OperationError { op: CompareOp }, -} - -impl From for PyErr { - fn from(err: PythonError) -> PyErr { - PyException::new_err(err.to_string()) - } -} - -impl From for PyErr { - fn from(err: TrajError) -> PyErr { - PyException::new_err(err.to_string()) - } -} - -impl From for PyErr { - fn from(err: ODError) -> PyErr { - PyException::new_err(err.to_string()) - } -} - -impl From for PyErr { - fn from(err: PropagationError) -> PyErr { - PyException::new_err(err.to_string()) - } -} - -impl From for PyErr { - fn from(err: AstroError) -> PyErr { - PyException::new_err(err.to_string()) - } -} - -impl From for PyErr { - fn from(err: NyxError) -> PyErr { - PyException::new_err(err.to_string()) - } -} - -impl From for PyErr { - fn from(err: ConfigError) -> PyErr { - PyException::new_err(err.to_string()) - } -} - -impl From for PyErr { - fn from(err: InputOutputError) -> PyErr { - PyException::new_err(err.to_string()) - } -} - -#[pymodule] -fn _nyx_space(py: Python, m: &PyModule) -> PyResult<()> { - pyo3_log::init(); - - register_time_module(py, m)?; - orbit_determination::register_od(py, m)?; - mission_design::register_md(py, m)?; - cosmic::register_cosmic(py, m)?; - monte_carlo::register_mc(py, m)?; - - Ok(()) -} - -/// Reexport hifitime as nyx_space.time -fn register_time_module(py: Python<'_>, parent_module: &PyModule) -> PyResult<()> { - let sm = PyModule::new(py, "_nyx_space.time")?; - - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - - py_run!(py, sm, "import sys; sys.modules['nyx_space.time'] = sm"); - parent_module.add_submodule(sm)?; - Ok(()) -} diff --git a/src/python/monte_carlo/mod.rs b/src/python/monte_carlo/mod.rs deleted file mode 100644 index 0aa23ada..00000000 --- a/src/python/monte_carlo/mod.rs +++ /dev/null @@ -1,115 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use crate::mc::GaussianGenerator; -use crate::md::StateParameter; -use crate::Orbit; -use crate::{NyxError, Spacecraft}; -use pyo3::{prelude::*, py_run}; -use rand::SeedableRng; -use rand_distr::Distribution; -use rand_pcg::Pcg64Mcg; - -/// Monte Carlo -pub(crate) fn register_mc(py: Python<'_>, parent_module: &PyModule) -> PyResult<()> { - let sm = PyModule::new(py, "_nyx_space.monte_carlo")?; - - sm.add_class::()?; - sm.add_function(wrap_pyfunction!(generate_orbits, sm)?)?; - sm.add_function(wrap_pyfunction!(generate_spacecraft, sm)?)?; - - py_run!( - py, - sm, - "import sys; sys.modules['nyx_space.monte_carlo'] = sm" - ); - parent_module.add_submodule(sm)?; - Ok(()) -} - -/// Generates orbits from the provided template orbit, the parameters to disperse, and whether these are absolute standard deviations or a percentage of the parameter's value. -#[pyfunction] -#[pyo3(text_signature = "(orbit, parameters, count, kind='abs', seed=None, /)")] -fn generate_orbits( - orbit: Orbit, - parameters: Vec<(StateParameter, f64)>, - count: usize, - kind: String, - seed: Option, -) -> Result, NyxError> { - let generator = match kind.as_str() { - "abs" => GaussianGenerator::from_3std_devs(orbit, ¶meters)?, - "prct" => GaussianGenerator::from_3std_dev_prcts(orbit, ¶meters)?, - _ => { - return Err(NyxError::CustomError { - msg: format!( - "Unknown kind of distribution: {} (should be 'abs' or 'prct')", - kind - ), - }) - } - }; - - let rng = match seed { - Some(seed) => Pcg64Mcg::new(seed.into()), - None => Pcg64Mcg::from_entropy(), - }; - - // Generate multithreaded - Ok(generator - .sample_iter(rng) - .take(count) - .map(|disp_state| disp_state.state) - .collect::>()) -} - -/// Generates spacecraft from the provided template spacecraft, the parameters to disperse, and whether these are absolute standard deviations or a percentage of the parameter's value. -#[pyfunction] -#[pyo3(text_signature = "(spacecraft, parameters, count, kind='abs', seed=None, /)")] -fn generate_spacecraft( - spacecraft: Spacecraft, - parameters: Vec<(StateParameter, f64)>, - count: usize, - kind: String, - seed: Option, -) -> Result, NyxError> { - let generator = match kind.as_str() { - "abs" => GaussianGenerator::from_3std_devs(spacecraft, ¶meters)?, - "prct" => GaussianGenerator::from_3std_dev_prcts(spacecraft, ¶meters)?, - _ => { - return Err(NyxError::CustomError { - msg: format!( - "Unknown kind of distribution: {} (should be 'abs' or 'prct')", - kind - ), - }) - } - }; - - let rng = match seed { - Some(seed) => Pcg64Mcg::new(seed.into()), - None => Pcg64Mcg::from_entropy(), - }; - - // Generate multithreaded - Ok(generator - .sample_iter(rng) - .take(count) - .map(|disp_state| disp_state.state) - .collect::>()) -} diff --git a/src/python/orbit_determination/arc.rs b/src/python/orbit_determination/arc.rs deleted file mode 100644 index 9daac06c..00000000 --- a/src/python/orbit_determination/arc.rs +++ /dev/null @@ -1,112 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use crate::cosmic::Cosm; -use crate::io::trajectory_data::TrajectoryLoader; -use crate::io::ExportCfg; -use crate::od::msr::RangeDoppler; -use crate::od::simulator::TrackingArcSim; -pub use crate::od::simulator::TrkConfig; -pub use crate::{io::ConfigError, od::prelude::GroundStation}; -use crate::{NyxError, Orbit, Spacecraft}; -use either::Either; -use pyo3::prelude::*; -use std::collections::BTreeMap; - -#[derive(Clone)] -#[pyclass] -pub struct GroundTrackingArcSim { - inner: Either< - TrackingArcSim, - TrackingArcSim, - >, -} - -#[pymethods] -impl GroundTrackingArcSim { - /// Initializes a new tracking arc simulation from the provided devices, trajectory, and the random number generator seed. - #[new] - pub fn with_seed( - devices: Vec, - trajectory: TrajectoryLoader, - configs: BTreeMap, - seed: u64, - ) -> Result { - // Try to convert the dynamic trajectory into a trajectory - let inner = if let Ok(sc_traj) = trajectory.to_traj::() { - let inner = TrackingArcSim::with_seed(devices, sc_traj, configs, seed)?; - - Either::Left(inner) - } else if let Ok(traj) = trajectory.to_traj::() { - let inner = TrackingArcSim::with_seed(devices, traj, configs, seed)?; - - Either::Right(inner) - } else { - return Err(ConfigError::InvalidConfig { - msg: "Trajectory could neither be parsed as an orbit nor spacecraft trajectory" - .to_string(), - }); - }; - - Ok(Self { inner }) - } - - /// Generates simulated measurements and returns the path where the parquet file containing said measurements is stored. - /// You may specify a metadata dictionary to be stored in the parquet file and whether the filename should include the timestamp. - pub fn generate_measurements( - &mut self, - path: String, - export_cfg: ExportCfg, - ) -> Result { - let cosm = Cosm::de438(); - let arc = match &mut self.inner { - Either::Left(arc_sim) => arc_sim.generate_measurements(cosm)?, - Either::Right(arc_sim) => arc_sim.generate_measurements(cosm)?, - }; - - // Save the tracking arc - let maybe = arc.to_parquet(path, export_cfg); - - match maybe { - Ok(path) => Ok(format!("{}", path.to_str().unwrap())), - Err(e) => Err(NyxError::CustomError { msg: e.to_string() }), - } - } - - /// Generates a tracking schedule - pub fn generate_schedule(&self) -> Result, NyxError> { - let cosm = Cosm::de438(); - match &self.inner { - Either::Left(arc_sim) => arc_sim.generate_schedule(cosm), - Either::Right(arc_sim) => arc_sim.generate_schedule(cosm), - } - } - - /// Builds a tracking schedule by generating it and storing it in this object. - pub fn build_schedule(&mut self) -> Result<(), NyxError> { - let cosm = Cosm::de438(); - match &mut self.inner { - Either::Left(arc_sim) => arc_sim.build_schedule(cosm), - Either::Right(arc_sim) => arc_sim.build_schedule(cosm), - } - } - - pub fn __repr__(&self) -> String { - format!("{}", self.inner) - } -} diff --git a/src/python/orbit_determination/estimate.rs b/src/python/orbit_determination/estimate.rs deleted file mode 100644 index 853d2963..00000000 --- a/src/python/orbit_determination/estimate.rs +++ /dev/null @@ -1,163 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use std::{collections::BTreeMap, sync::Arc}; - -use crate::python::PythonError; -use crate::{ - cosmic::Cosm, - io::{estimate::OrbitEstimateSerde, ConfigRepr, Configurable}, - od::estimate::KfEstimate, - NyxError, Orbit, -}; -use nalgebra::Matrix6; -use numpy::{PyReadonlyArrayDyn, PyUntypedArrayMethods}; -use pyo3::class::basic::CompareOp; -use pyo3::prelude::*; -use pyo3::types::PyType; -use pythonize::depythonize; - -use super::ConfigError; - -/// An estimate of an orbit with its covariance, the latter should be a numpy array of size 36. -#[derive(Debug, Clone, PartialEq)] -#[pyclass] -pub(crate) struct OrbitEstimate(pub(crate) KfEstimate); - -impl Configurable for OrbitEstimate { - type IntermediateRepr = OrbitEstimateSerde; - - fn from_config(cfg: Self::IntermediateRepr, _almanac: Arc) -> Result - where - Self: Sized, - { - Ok(Self(KfEstimate::from_covar( - Orbit::from(cfg.nominal), - cfg.covar.to_matrix(), - ))) - } - - fn to_config(&self) -> Result { - todo!() - } -} - -#[pymethods] -impl OrbitEstimate { - #[new] - #[pyo3(text_signature = "(nominal_orbit, covariance)")] - fn new(nominal: Orbit, covar: PyReadonlyArrayDyn) -> Result { - // Check the shape of the input - let mat6 = match covar.shape() { - &[36] | &[36, 1] | &[6, 6] => { - let data = covar.as_slice().map_err(|e| NyxError::CustomError { - msg: format!("{e}"), - })?; - let mut mat = Matrix6::zeros(); - for i in 0..6 { - for j in 0..6 { - mat[(i, j)] = data[6 * i + j]; - } - } - mat - } - _ => { - return Err(NyxError::CustomError { - msg: format!("covar must be 6x6 or 36x1 but is {:?}", covar.shape()), - }) - } - }; - Ok(Self(KfEstimate::from_covar(nominal, mat6))) - } - - #[classmethod] - fn load(_cls: &PyType, path: &str) -> Result { - let serde = OrbitEstimateSerde::load(path)?; - - let cosm = Cosm::de438(); - - Self::from_config(serde, cosm) - } - - #[classmethod] - fn load_many(_cls: &PyType, path: &str) -> Result, ConfigError> { - let stations = OrbitEstimateSerde::load_many(path)?; - - let cosm = Cosm::de438(); - - let mut selves = Vec::with_capacity(stations.len()); - - for serde in stations { - selves.push(Self::from_config(serde, cosm.clone())?); - } - - Ok(selves) - } - - #[classmethod] - fn load_named(_cls: &PyType, path: &str) -> Result, ConfigError> { - let orbits = OrbitEstimateSerde::load_named(path)?; - - let cosm = Cosm::de438(); - - let mut selves = BTreeMap::new(); - - for (k, v) in orbits { - selves.insert(k, Self::from_config(v, cosm.clone())?); - } - - Ok(selves) - } - - fn __richcmp__(&self, other: &Self, op: CompareOp) -> Result { - match op { - CompareOp::Eq => Ok(self.__repr__() == other.__repr__()), - CompareOp::Ne => Ok(self.__repr__() != other.__repr__()), - _ => Err(PythonError::OperationError { op }), - } - } - - #[classmethod] - /// Loads the SpacecraftDynamics from its YAML representation - fn loads(_cls: &PyType, state: &PyAny) -> Result { - ::from_config( - depythonize(state).map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() })?, - Cosm::de438(), - ) - } - - // Manual getter/setters -- waiting on https://github.com/PyO3/pyo3/pull/2786 - - #[getter] - fn get_orbit(&self) -> PyResult { - Ok(self.0.nominal_state) - } - - #[getter] - fn get_is_predicted(&self) -> PyResult { - Ok(self.0.predicted) - } - - fn __str__(&self) -> String { - format!("{}", self.0) - } - - fn __repr__(&self) -> String { - format!("{:?}", self.0) - } -} diff --git a/src/python/orbit_determination/ground_station.rs b/src/python/orbit_determination/ground_station.rs deleted file mode 100644 index c85d2610..00000000 --- a/src/python/orbit_determination/ground_station.rs +++ /dev/null @@ -1,271 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use std::collections::BTreeMap; - -use crate::cosmic::{Cosm, Orbit}; -use crate::io::{ConfigRepr, ParseSnafu}; -use crate::od::simulator::TrackingDeviceSim; -pub use crate::od::simulator::TrkConfig; -use crate::od::ODError; -use crate::python::PythonError; -use crate::time::Duration; -use crate::NyxError; -pub use crate::{io::ConfigError, od::noise::GaussMarkov, od::prelude::GroundStation}; - -use crate::python::cosmic::Cosm as CosmPy; -use crate::python::pyo3utils::pyany_to_value; - -use pyo3::class::basic::CompareOp; -use pyo3::prelude::*; -use pyo3::types::{PyDict, PyList, PyType}; -use pythonize::pythonize; -use snafu::ResultExt; - -#[pymethods] -impl GroundStation { - #[new] - fn new( - name: String, - elevation_mask_deg: f64, - latitude_deg: f64, - longitude_deg: f64, - height_km: f64, - frame: String, - light_time_correction: bool, - integration_time: Option, - timestamp_noise_s: Option, - range_noise_km: Option, - doppler_noise_km_s: Option, - ) -> Result { - let cosm = Cosm::de438(); - let frame_obj = cosm.try_frame(&frame)?; - Ok(Self { - name, - elevation_mask_deg, - latitude_deg, - longitude_deg, - height_km, - frame: frame_obj, - integration_time, - light_time_correction, - timestamp_noise_s, - range_noise_km, - doppler_noise_km_s, - }) - } - - fn __getnewargs__( - &self, - ) -> Result< - ( - String, - f64, - f64, - f64, - f64, - String, - bool, - Option, - Option, - Option, - Option, - ), - NyxError, - > { - Ok(( - self.name.clone(), - self.elevation_mask_deg, - self.latitude_deg, - self.longitude_deg, - self.height_km, - self.frame.to_string(), - self.light_time_correction, - self.integration_time, - self.timestamp_noise_s, - self.range_noise_km, - self.doppler_noise_km_s, - )) - } - - #[classmethod] - fn load(_cls: &PyType, path: &str) -> Result { - ::load(path) - } - - #[classmethod] - fn load_many(_cls: &PyType, path: &str) -> Result, ConfigError> { - ::load_many(path) - } - - #[classmethod] - fn load_named(_cls: &PyType, path: &str) -> Result, ConfigError> { - ::load_named(path) - } - - #[classmethod] - /// Loads the SpacecraftDynamics from its YAML representation - fn loads(_cls: &PyType, data: &PyAny) -> Result, ConfigError> { - if let Ok(as_list) = data.downcast::() { - let mut as_map = BTreeMap::new(); - for item in as_list.iter() { - // Check that the item is a dictionary - let next: Self = - serde_yml::from_value(pyany_to_value(item)?).context(ParseSnafu)?; - as_map.insert(next.name.clone(), next); - } - Ok(as_map) - } else if let Ok(as_dict) = data.downcast::() { - let mut as_map = BTreeMap::new(); - for item_as_list in as_dict.items() { - let k_any = item_as_list.get_item(0).or_else(|_| { - Err(ConfigError::InvalidConfig { - msg: "could not get key from provided dictionary item".to_string(), - }) - })?; - let v_any = item_as_list.get_item(1).or_else(|_| { - Err(ConfigError::InvalidConfig { - msg: "could not get key from provided dictionary item".to_string(), - }) - })?; - // Check that it's a string, or abort here - if let Ok(as_str) = k_any.extract::() { - // Try to convert the underlying data - match pyany_to_value(v_any) { - Ok(value) => { - let next: Self = serde_yml::from_value(value).context(ParseSnafu)?; - as_map.insert(as_str, next); - } - Err(_) => { - // Maybe this was to be parsed in full as a single item - let me: Self = serde_yml::from_value(pyany_to_value(as_dict)?) - .context(ParseSnafu)?; - as_map.clear(); - as_map.insert(me.name.clone(), me); - return Ok(as_map); - } - } - } else { - return Err(ConfigError::InvalidConfig { - msg: - "keys for `loads` must be strings, otherwise, use GroundStation(**data)" - .to_string(), - }); - } - } - Ok(as_map) - } else { - Err(ConfigError::InvalidConfig { - msg: "config must be dict, list, or str".to_string(), - }) - } - } - - fn dumps(&self, py: Python) -> Result { - pythonize(py, &self).map_err(|e| NyxError::CustomError { msg: e.to_string() }) - } - - /// Perform a one-way measurement of the given orbit at the epoch stored in that orbit instance. - /// Returns the range in kilometers and the Doppler measurement in kilometers per second. - fn measure(&mut self, orbit: Orbit) -> Result, ODError> { - match self.measure_instantaneous(orbit, None, Cosm::de438())? { - Some(msr) => Ok(Some((msr.obs[0], msr.obs[1]))), - None => Ok(None), - } - } - - /// Computes the azimuth and elevation of the provided object seen from this ground station, both in degrees. - fn compute_azimuth_elevation(&self, receiver: Orbit, cosm: &CosmPy) -> (f64, f64) { - let (az_deg, el_deg, _, _) = self.azimuth_elevation_of(receiver, &cosm.inner); - - (az_deg, el_deg) - } - - // Manual getter/setters -- waiting on https://github.com/PyO3/pyo3/pull/2786 - - #[getter] - fn get_name(&self) -> PyResult { - Ok(self.name.clone()) - } - - #[setter] - fn set_name(&mut self, name: String) -> PyResult<()> { - self.name = name; - Ok(()) - } - - #[getter] - fn get_elevation_mask_deg(&self) -> PyResult { - Ok(self.elevation_mask_deg) - } - - #[setter] - fn set_elevation_mask_deg(&mut self, mask_deg: f64) -> PyResult<()> { - self.elevation_mask_deg = mask_deg; - Ok(()) - } - - #[getter] - fn get_latitude_deg(&self) -> PyResult { - Ok(self.latitude_deg) - } - - #[setter] - fn set_latitude_deg(&mut self, lat_deg: f64) -> PyResult<()> { - self.latitude_deg = lat_deg; - Ok(()) - } - - #[getter] - fn get_longitude_deg(&self) -> PyResult { - Ok(self.longitude_deg) - } - - #[setter] - fn set_longitude_deg(&mut self, long_deg: f64) -> PyResult<()> { - self.longitude_deg = long_deg; - Ok(()) - } - - #[getter] - fn get_height_km(&self) -> PyResult { - Ok(self.height_km) - } - - #[setter] - fn set_height_km(&mut self, height_km: f64) -> PyResult<()> { - self.height_km = height_km; - Ok(()) - } - - fn __repr__(&self) -> String { - format!("{self:?}") - } - - fn __str__(&self) -> String { - format!("{self}") - } - - fn __richcmp__(&self, other: &Self, op: CompareOp) -> Result { - match op { - CompareOp::Eq => Ok(self == other), - CompareOp::Ne => Ok(self != other), - _ => Err(PythonError::OperationError { op }), - } - } -} diff --git a/src/python/orbit_determination/mod.rs b/src/python/orbit_determination/mod.rs deleted file mode 100644 index a84cd17b..00000000 --- a/src/python/orbit_determination/mod.rs +++ /dev/null @@ -1,60 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use crate::io::tracking_data::DynamicTrackingArc; -use crate::io::ExportCfg; -use crate::od::noise::GaussMarkov; -use crate::od::process::{FltResid, IterationConf}; -pub use crate::od::simulator::{Scheduler, Strand, TrkConfig}; -pub use crate::{io::ConfigError, od::prelude::GroundStation}; -use pyo3::{prelude::*, py_run}; -mod arc; -pub(crate) mod estimate; -mod ground_station; -mod process; -mod scheduler; -mod trkconfig; - -use estimate::OrbitEstimate; -use process::{predictor, process_tracking_arc}; - -pub(crate) fn register_od(py: Python<'_>, parent_module: &PyModule) -> PyResult<()> { - let sm = PyModule::new(py, "_nyx_space.orbit_determination")?; - - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_class::()?; - sm.add_function(wrap_pyfunction!(process_tracking_arc, sm)?)?; - sm.add_function(wrap_pyfunction!(predictor, sm)?)?; - - py_run!( - py, - sm, - "import sys; sys.modules['nyx_space.orbit_determination'] = sm" - ); - parent_module.add_submodule(sm)?; - Ok(()) -} diff --git a/src/python/orbit_determination/process.rs b/src/python/orbit_determination/process.rs deleted file mode 100644 index 0307caae..00000000 --- a/src/python/orbit_determination/process.rs +++ /dev/null @@ -1,181 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use hifitime::{Duration, Epoch}; -use nalgebra::Matrix2; -use pyo3::prelude::*; -use snafu::ResultExt; - -use crate::{ - io::tracking_data::DynamicTrackingArc, - io::ExportCfg, - md::prelude::{Cosm, Propagator, SpacecraftDynamics}, - od::{ - filter::kalman::KF, - process::{EkfTrigger, FltResid, IterationConf, ODIOSnafu, ODProcess}, - snc::SNC3, - ODError, - }, - Spacecraft, -}; - -use super::{estimate::OrbitEstimate, ConfigError, GroundStation}; - -/// Runs an orbit determination process and returns the path to those results. -#[pyfunction] -pub(crate) fn process_tracking_arc( - dynamics: SpacecraftDynamics, - spacecraft: Spacecraft, - initial_estimate: OrbitEstimate, - measurement_noise: Vec, - arc: &DynamicTrackingArc, - export_path: String, - export_cfg: Option, - ekf_num_meas: Option, - ekf_disable_time: Option, - resid_crit: Option, - predict_until: Option, - predict_for: Option, - predict_step: Option, - iter_conf: Option, - snc_disable_time: Option, - snc_diagonals: Option>, -) -> Result { - let msr_noise = Matrix2::from_iterator(measurement_noise); - - let init_sc = spacecraft.with_orbit(initial_estimate.0.nominal_state.with_stm()); - - // Build KF without SNC - let kf = if (snc_disable_time.is_some() && snc_diagonals.as_ref().is_none()) - || (snc_disable_time.is_none() && snc_diagonals.as_ref().is_some()) - || (snc_diagonals.as_ref().is_some() && snc_diagonals.as_ref().unwrap().len() != 3) - { - return Err(ODError::ODConfigError { - source: ConfigError::InvalidConfig { - msg: "SNC requires a disable time and the snc_diagonals (3 items required)." - .to_string(), - }, - }); - } else if snc_disable_time.is_some() && snc_diagonals.is_some() { - let snc = SNC3::from_diagonal(snc_disable_time.unwrap(), &snc_diagonals.unwrap()); - KF::new(initial_estimate.0, snc, msr_noise) - } else { - KF::no_snc(initial_estimate.0, msr_noise) - }; - - let prop = Propagator::default(dynamics); - let prop_est = prop.with(init_sc); - - if (ekf_disable_time.is_some() && ekf_num_meas.is_none()) - || (ekf_disable_time.is_none() && ekf_num_meas.is_some()) - { - return Err(ODError::ODConfigError { - source: ConfigError::InvalidConfig { - msg: "For an EKF trigger, you must provide both a disable time and a num measurements." - .to_string(), - }, - }); - } - - let trigger = match ekf_num_meas { - Some(ekf_num_meas) => Some(EkfTrigger::new(ekf_num_meas, ekf_disable_time.unwrap())), - None => None, - }; - - let mut odp = ODProcess::new(prop_est, kf, trigger, resid_crit, Cosm::de438()); - - let concrete_arc = arc.to_tracking_arc().context(ODIOSnafu)?; - - odp.process_arc::(&concrete_arc)?; - - if let Some(iter_conf) = iter_conf { - odp.iterate_arc::(&concrete_arc, iter_conf)?; - } - - if let Some(epoch) = predict_until { - let max_step = predict_step.ok_or_else(|| ODError::ODConfigError { - source: ConfigError::InvalidConfig { - msg: "predict_step unset.".to_string(), - }, - })?; - odp.predict_until(max_step, epoch)?; - } else if let Some(duration) = predict_for { - let max_step = predict_step.ok_or_else(|| ODError::ODConfigError { - source: ConfigError::InvalidConfig { - msg: "predict_step unset.".to_string(), - }, - })?; - odp.predict_for(max_step, duration)?; - } - - let path = odp.to_parquet( - export_path, - export_cfg.unwrap_or_else(|| ExportCfg::default()), - )?; - - Ok(format!("{}", path.to_str().unwrap())) -} - -/// Runs an orbit determination prediction-only process and returns the path to those results. -#[pyfunction] -pub(crate) fn predictor( - dynamics: SpacecraftDynamics, - spacecraft: Spacecraft, - initial_estimate: OrbitEstimate, - step: Duration, - export_path: String, - export_cfg: Option, - predict_until: Option, - predict_for: Option, -) -> Result { - // TODO: Return a navigation trajectory or use a class that mimics the better ODProcess -- https://github.com/nyx-space/nyx/issues/134 - let msr_noise = Matrix2::from_iterator(vec![1e-10, 0.0, 0.0, 1e-10]); - - let init_sc = spacecraft.with_orbit(initial_estimate.0.nominal_state.with_stm()); - - // Build KF without SNC - let kf = KF::no_snc(initial_estimate.0, msr_noise); - - let prop = Propagator::default(dynamics); - let prop_est = prop.with(init_sc); - - if (predict_until.is_some() && predict_for.is_some()) - || (predict_until.is_none() && predict_for.is_none()) - { - return Err(ODError::ODConfigError { - source: ConfigError::InvalidConfig { - msg: "predict_step and predict_for unset.".to_string(), - }, - }); - } - - let mut odp = ODProcess::ckf(prop_est, kf, None, Cosm::de438()); - - if let Some(epoch) = predict_until { - odp.predict_until(step, epoch)?; - } else if let Some(duration) = predict_for { - odp.predict_for(step, duration)?; - } - - let path = odp.to_parquet( - export_path, - export_cfg.unwrap_or_else(|| ExportCfg::default()), - )?; - - Ok(format!("{}", path.to_str().unwrap())) -} diff --git a/src/python/orbit_determination/scheduler.rs b/src/python/orbit_determination/scheduler.rs deleted file mode 100644 index e3b7fb79..00000000 --- a/src/python/orbit_determination/scheduler.rs +++ /dev/null @@ -1,80 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ -pub use crate::io::ConfigError; -pub use crate::od::simulator::{Cadence, Handoff, Scheduler, Strand}; -use crate::python::PythonError; - -use hifitime::Duration; -use pyo3::basic::CompareOp; -use pyo3::prelude::*; -use std::str::FromStr; - -#[pymethods] -impl Scheduler { - #[new] - #[pyo3(text_signature = "(handoff, cadence_on=None, cadence_off=None)")] - fn py_new( - handoff: Handoff, - cadence_on: Option, - cadence_off: Option, - min_samples: Option, - ) -> Result { - let mut me = Self::builder().handoff(handoff).build(); - - if let Some(min_samples) = min_samples { - me.min_samples = min_samples; - } - - if cadence_on.is_some() || cadence_off.is_some() { - me.cadence = Cadence::Intermittent { - on: Duration::from_str(cadence_on.unwrap().as_str()).map_err(|e| { - ConfigError::InvalidConfig { - msg: format!( - "{e} invalid format for schedule on (must be specified if schedule off is)" - ), - } - })?, - off: Duration::from_str(cadence_off.unwrap().as_str()).map_err(|e| { - ConfigError::InvalidConfig { - msg: format!( - "{e} invalid format for schedule off (must be specified if schedule on is)" - ), - } - })?, - }; - } - - Ok(me) - } - - fn __repr__(&self) -> String { - format!("{self:?}") - } - - fn __str__(&self) -> String { - format!("{self:?}") - } - - fn __richcmp__(&self, other: &Self, op: CompareOp) -> Result { - match op { - CompareOp::Eq => Ok(self == other), - CompareOp::Ne => Ok(self != other), - _ => Err(PythonError::OperationError { op }), - } - } -} diff --git a/src/python/orbit_determination/trkconfig.rs b/src/python/orbit_determination/trkconfig.rs deleted file mode 100644 index b0fee72e..00000000 --- a/src/python/orbit_determination/trkconfig.rs +++ /dev/null @@ -1,119 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -pub use crate::io::ConfigError; -pub use crate::od::simulator::{Scheduler, Strand, TrkConfig}; -use crate::python::PythonError; -use crate::{io::ConfigRepr, NyxError}; -use hifitime::Duration; -use pyo3::basic::CompareOp; -use pyo3::prelude::*; -use pyo3::types::PyType; -use pythonize::{depythonize, pythonize}; -use std::collections::BTreeMap; -use std::str::FromStr; - -#[pymethods] -impl TrkConfig { - #[classmethod] - fn load(_cls: &PyType, path: &str) -> Result { - ::load(path) - } - - #[classmethod] - fn load_many(_cls: &PyType, path: &str) -> Result, ConfigError> { - ::load_many(path) - } - - #[classmethod] - fn load_named(_cls: &PyType, path: &str) -> Result, ConfigError> { - ::load_named(path) - } - - #[new] - #[pyo3(text_signature = "(sampling=None, strands=None, scheduler=None)")] - fn py_new( - sampling: Option, - strands: Option>, - scheduler: Option, - ) -> Result { - let mut me = Self::default(); - - if let Some(sampling) = sampling { - me.sampling = - Duration::from_str(&sampling).map_err(|e| ConfigError::InvalidConfig { - msg: format!("{e} invalid format for sampling"), - })?; - } - - me.strands = strands; - - if scheduler.is_some() { - me.scheduler = scheduler; - } - - Ok(me) - } - - fn __repr__(&self) -> String { - serde_yml::to_string(&self).unwrap() - } - - fn __str__(&self) -> String { - format!("{self:?}") - } - - fn __richcmp__(&self, other: &Self, op: CompareOp) -> Result { - match op { - CompareOp::Eq => Ok(self == other), - CompareOp::Ne => Ok(self != other), - _ => Err(PythonError::OperationError { op }), - } - } - - fn dumps(&self, py: Python) -> Result { - pythonize(py, &self).map_err(|e| NyxError::CustomError { msg: e.to_string() }) - } - - fn __getstate__(&self, py: Python) -> Result { - self.dumps(py) - } - - fn __setstate__(&mut self, state: &PyAny) -> Result<(), ConfigError> { - *self = - depythonize(state).map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() })?; - Ok(()) - } - - #[classmethod] - /// Loads the TrkConfig from its YAML representation - fn loads(_cls: &PyType, state: &PyAny) -> Result { - depythonize(state).map_err(|e| ConfigError::InvalidConfig { msg: e.to_string() }) - } - - #[getter] - fn get_sampling(&self) -> PyResult { - Ok(self.sampling) - } - - #[setter] - fn set_sampling(&mut self, sampling: Duration) -> PyResult<()> { - self.sampling = sampling; - Ok(()) - } -} diff --git a/src/python/pyo3utils/mod.rs b/src/python/pyo3utils/mod.rs deleted file mode 100644 index 146abeb1..00000000 --- a/src/python/pyo3utils/mod.rs +++ /dev/null @@ -1,63 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU Affero General Public License as published - by the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Affero General Public License for more details. - - You should have received a copy of the GNU Affero General Public License - along with this program. If not, see . -*/ - -use crate::io::ConfigError; -use pyo3::types::{PyAny, PyDict, PyList}; -use serde_yml::{Mapping, Value}; - -/// Try to convert the provided PyAny into a SerDe YAML Value -pub fn pyany_to_value(any: &PyAny) -> Result { - if let Ok(as_bool) = any.extract::() { - Ok(Value::Bool(as_bool)) - } else if let Ok(as_str) = any.extract::() { - Ok(Value::String(as_str)) - } else if let Ok(as_f64) = any.extract::() { - Ok(Value::Number(as_f64.into())) - } else if let Ok(as_int) = any.extract::() { - Ok(Value::Number(as_int.into())) - } else if let Ok(as_int) = any.extract::() { - Ok(Value::Number(as_int.into())) - } else if let Ok(as_list) = any.downcast::() { - let mut seq = Vec::new(); - for item in as_list.iter() { - seq.push(pyany_to_value(item)?); - } - Ok(Value::Sequence(seq)) - } else if let Ok(as_dict) = any.downcast::() { - let mut map = Mapping::new(); - for item_as_list in as_dict.items() { - let k = pyany_to_value(item_as_list.get_item(0).or_else(|_| { - Err(ConfigError::InvalidConfig { - msg: "could not get key from provided dictionary item".to_string(), - }) - })?)?; - let v = pyany_to_value(item_as_list.get_item(1).or_else(|_| { - Err(ConfigError::InvalidConfig { - msg: "could not get value from provided dictionary item".to_string(), - }) - })?)?; - map.insert(k, v); - } - - Ok(Value::Mapping(map)) - } else { - Err(ConfigError::InvalidConfig { - msg: "cannot convert input (not one of bool, int, float, str, List, Dict)".to_string(), - }) - } -} diff --git a/tests/mission_design/multishoot/mod.rs b/tests/mission_design/multishoot/mod.rs index c7511a56..9e3a26b2 100644 --- a/tests/mission_design/multishoot/mod.rs +++ b/tests/mission_design/multishoot/mod.rs @@ -13,6 +13,7 @@ use anise::{ }, prelude::Almanac, }; +use nyx_space::cosmic::Mass; use rstest::*; use std::sync::Arc; @@ -36,8 +37,7 @@ fn alt_orbit_raising_cov_test(almanac: Arc) { /* Build the spacecraft -- really only the mass is needed here */ let sc = Spacecraft { orbit: start, - dry_mass_kg: 100.0, - fuel_mass_kg: 500.0, + mass: Mass::from_dry_and_prop_masses(100.0, 500.0), thruster: Some(Thruster { thrust_N: 150.0, isp_s: 300.0, @@ -224,8 +224,7 @@ fn vmag_orbit_raising(almanac: Arc) { /* Build the spacecraft -- really only the mass is needed here */ let sc = Spacecraft { orbit: start, - dry_mass_kg: 100.0, - fuel_mass_kg: 500.0, + mass: Mass::from_dry_and_prop_masses(100.0, 500.0), thruster: Some(Thruster { thrust_N: 150.0, isp_s: 300.0, diff --git a/tests/mission_design/orbitaldyn.rs b/tests/mission_design/orbitaldyn.rs index b100db77..0d7637c8 100644 --- a/tests/mission_design/orbitaldyn.rs +++ b/tests/mission_design/orbitaldyn.rs @@ -838,11 +838,11 @@ fn multi_body_dynamics_dual(almanac: Arc) { ); // This should be zero! assert!( - err_r < 2e-16, + err_r < f64::EPSILON, "position error too large for multibody gravity" ); assert!( - err_v < 2e-16, + err_v < f64::EPSILON, "velocity error too large for multibody gravity" ); @@ -1020,8 +1020,14 @@ fn val_earth_sph_harmonics_12x12(almanac_gmat: Arc) { err_v * 1e3 ); // This should be zero! - assert!(err_r < 2e-16, "position error too large for 12x12 gravity"); - assert!(err_v < 2e-16, "velocity error too large for 12x12 gravity"); + assert!( + err_r < f64::EPSILON, + "position error too large for 12x12 gravity" + ); + assert!( + err_v < f64::EPSILON, + "velocity error too large for 12x12 gravity" + ); } #[allow(clippy::identity_op)] diff --git a/tests/mission_design/targeter/finite_burns.rs b/tests/mission_design/targeter/finite_burns.rs index e330e3d0..6f3aba35 100644 --- a/tests/mission_design/targeter/finite_burns.rs +++ b/tests/mission_design/targeter/finite_burns.rs @@ -2,9 +2,10 @@ extern crate nyx_space as nyx; use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SUN}; use hifitime::TimeUnits; -use nyx::dynamics::guidance::{LocalFrame, Mnvr, Thruster}; +use nyx::dynamics::guidance::{LocalFrame, Maneuver, Thruster}; use nyx::linalg::Vector3; use nyx::md::prelude::*; +use nyx_space::cosmic::Mass; use crate::propagation::GMAT_EARTH_GM; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; @@ -31,8 +32,7 @@ fn thrust_dir_tgt_sma_aop_raan(almanac: Arc) { let spacecraft = Spacecraft { orbit: xi_orig, - dry_mass_kg: 10.0, - fuel_mass_kg: 90.0, + mass: Mass::from_dry_and_prop_masses(10.0, 90.0), thruster: Some(Thruster { thrust_N: 500.0, isp_s: 300.0, @@ -78,8 +78,7 @@ fn thrust_dir_rate_tgt_sma_aop_raan(almanac: Arc) { let spacecraft = Spacecraft { orbit: xi_orig, - dry_mass_kg: 10.0, - fuel_mass_kg: 90.0, + mass: Mass::from_dry_and_prop_masses(10.0, 90.0), thruster: Some(Thruster { thrust_N: 500.0, isp_s: 300.0, @@ -127,8 +126,7 @@ fn thrust_profile_tgt_sma_aop_raan_cov_test(almanac: Arc) { let spacecraft = Spacecraft { orbit: xi_orig, - dry_mass_kg: 10.0, - fuel_mass_kg: 90.0, + mass: Mass::from_dry_and_prop_masses(10.0, 90.0), thruster: Some(Thruster { thrust_N: 500.0, isp_s: 300.0, @@ -196,7 +194,7 @@ fn val_tgt_finite_burn(almanac: Arc) { // With 100% thrust: RSS errors: pos = 3.14651e1 km vel = 3.75245e-2 km/s // Define the maneuver and its schedule - let mnvr0 = Mnvr::from_time_invariant( + let mnvr0 = Maneuver::from_time_invariant( start_time + 1.seconds(), start_time + prop_time - 1.seconds(), 1.0, // Full thrust diff --git a/tests/mission_design/targeter/multi_oe.rs b/tests/mission_design/targeter/multi_oe.rs index c9229fcc..ab968ed4 100644 --- a/tests/mission_design/targeter/multi_oe.rs +++ b/tests/mission_design/targeter/multi_oe.rs @@ -4,6 +4,7 @@ use nyx::dynamics::guidance::Thruster; use nyx::md::prelude::*; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; +use nyx_space::cosmic::Mass; use rstest::*; use std::sync::Arc; @@ -84,8 +85,11 @@ fn conv_tgt_sma_ecc(almanac: Arc) { let spacecraft = Spacecraft { orbit: xi_orig, - dry_mass_kg: 10.0, - fuel_mass_kg: 90.0, + mass: Mass { + dry_mass_kg: 10.0, + prop_mass_kg: 90.0, + extra_mass_kg: 0.0, + }, thruster: Some(Thruster { thrust_N: 500.0, isp_s: 300.0, diff --git a/tests/orbit_determination/multi_body.rs b/tests/orbit_determination/multi_body.rs index a9b8e1c9..48534b64 100644 --- a/tests/orbit_determination/multi_body.rs +++ b/tests/orbit_determination/multi_body.rs @@ -175,7 +175,7 @@ fn od_val_multi_body_ckf_perfect_stations( ); } assert!( - est.state_deviation().norm() < 2e-16, + est.state_deviation().norm() < f64::EPSILON, "estimate error should be very good (perfect dynamics) ({:e})", est.state_deviation().norm() ); @@ -185,14 +185,14 @@ fn od_val_multi_body_ckf_perfect_stations( for res in odp.residuals.iter().flatten() { assert!( - res.postfit.norm() < 2e-16, + res.postfit.norm() < f64::EPSILON, "postfit should be zero (perfect dynamics) ({:e})", res ); } let est = last_est.unwrap(); - assert!(est.state_deviation().norm() < 2e-16); + assert!(est.state_deviation().norm() < f64::EPSILON); assert!(est.covar.norm() < 1e-5); let delta = (est.state().orbit - final_truth.orbit).unwrap(); @@ -202,8 +202,14 @@ fn od_val_multi_body_ckf_perfect_stations( delta.vmag_km_s() * 1e6 ); - assert!(delta.rmag_km() < 2e-16, "Position error should be zero"); - assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); + assert!( + delta.rmag_km() < f64::EPSILON, + "Position error should be zero" + ); + assert!( + delta.vmag_km_s() < f64::EPSILON, + "Velocity error should be zero" + ); } #[allow(clippy::identity_op)] @@ -295,7 +301,7 @@ fn multi_body_ckf_covar_map_cov_test( } else { // Only check that the covariance is low IF this isn't a predicted estimate assert!( - est.state_deviation().norm() < 2e-16, + est.state_deviation().norm() < f64::EPSILON, "estimate error should be zero (perfect dynamics) ({:e})", est.state_deviation().norm() ); @@ -313,7 +319,7 @@ fn multi_body_ckf_covar_map_cov_test( // Note that we check the residuals separately from the estimates because we have many predicted estimates which do not have any associated residuals. for res in odp.residuals.iter().flatten() { assert!( - res.postfit.norm() < 2e-16, + res.postfit.norm() < f64::EPSILON, "postfit should be zero (perfect dynamics) ({:e})", res ); diff --git a/tests/orbit_determination/robust.rs b/tests/orbit_determination/robust.rs index 67102a5f..04b6a438 100644 --- a/tests/orbit_determination/robust.rs +++ b/tests/orbit_determination/robust.rs @@ -396,7 +396,6 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { // Read in the Parquet file and assert proper data was written. - // let df = LazyFrame::scan_parquet(timestamped_path, Default::default()).unwrap(); let df = ParquetReader::new(File::open(timestamped_path).unwrap()) .finish() .unwrap(); diff --git a/tests/orbit_determination/spacecraft.rs b/tests/orbit_determination/spacecraft.rs index 751e3680..464960bb 100644 --- a/tests/orbit_determination/spacecraft.rs +++ b/tests/orbit_determination/spacecraft.rs @@ -12,7 +12,7 @@ use nyx::md::{Event, StateParameter}; use nyx::od::prelude::*; use nyx::propagators::{IntegratorOptions, Propagator}; use nyx::time::{Epoch, TimeUnits, Unit}; -use nyx_space::cosmic::SrpConfig; +use nyx_space::cosmic::{Mass, SRPData}; use nyx_space::dynamics::guidance::LocalFrame; use nyx_space::propagators::{ErrorControl, IntegratorMethod}; use std::collections::BTreeMap; @@ -323,11 +323,11 @@ fn od_val_sc_srp_estimation_cov_test( let sc_truth = Spacecraft::builder() .orbit(initial_orbit) - .srp(SrpConfig { - cr: truth_cr, + .srp(SRPData { + coeff_reflectivity: truth_cr, area_m2: 100.0, }) - .dry_mass_kg(dry_mass_kg) + .mass(Mass::from_dry_mass(dry_mass_kg)) .build(); println!("{sc_truth}"); @@ -381,11 +381,11 @@ fn od_val_sc_srp_estimation_cov_test( let sc_init_est = Spacecraft::builder() .orbit(initial_orbit) - .srp(SrpConfig { - cr: 1.5, + .srp(SRPData { + coeff_reflectivity: 1.5, area_m2: 100.0, }) - .dry_mass_kg(dry_mass_kg) + .mass(Mass::from_dry_mass(dry_mass_kg)) .build() .with_stm(); @@ -401,7 +401,7 @@ fn od_val_sc_srp_estimation_cov_test( .vx_km_s(0.5e-1) .vy_km_s(0.5e-1) .vz_km_s(0.5e-1) - .cr(0.2) + .coeff_reflectivity(0.2) .build(); // Define the initial orbit estimate @@ -437,7 +437,7 @@ fn od_val_sc_srp_estimation_cov_test( println!( "FINAL:\n\t{est}\n{:x}\nCr = {} +/-{}\nEXP:\t{:x}", est.state().orbit, - est.state().srp.cr, + est.state().srp.coeff_reflectivity, est.covar()[(6, 6)].sqrt(), truth.orbit ); @@ -452,7 +452,7 @@ fn od_val_sc_srp_estimation_cov_test( assert!(delta.rmag_km() < 2e-3, "More than 2 meter error"); assert!(delta.vmag_km_s() < 2e-6, "More than 2 millimeter/s error"); assert!( - (est.state().srp.cr - truth_cr).abs() < (1.5 - truth_cr), + (est.state().srp.coeff_reflectivity - truth_cr).abs() < (1.5 - truth_cr), "Cr estimation did not improve" ); diff --git a/tests/orbit_determination/two_body.rs b/tests/orbit_determination/two_body.rs index 96b6aa9e..079c3ee8 100644 --- a/tests/orbit_determination/two_body.rs +++ b/tests/orbit_determination/two_body.rs @@ -224,8 +224,14 @@ fn od_tb_val_ekf_fixed_step_perfect_stations( delta.vmag_km_s() * 1e6 ); - assert!(delta.rmag_km() < 2e-16, "Position error should be zero"); - assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); + assert!( + delta.rmag_km() < f64::EPSILON, + "Position error should be zero" + ); + assert!( + delta.vmag_km_s() < f64::EPSILON, + "Velocity error should be zero" + ); } #[allow(clippy::identity_op)] @@ -382,8 +388,14 @@ fn od_tb_val_with_arc( delta.vmag_km_s() * 1e6 ); - assert!(delta.rmag_km() < 2e-16, "Position error should be zero"); - assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); + assert!( + delta.rmag_km() < f64::EPSILON, + "Position error should be zero" + ); + assert!( + delta.vmag_km_s() < f64::EPSILON, + "Velocity error should be zero" + ); } #[fixture] @@ -568,8 +580,14 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( delta.vmag_km_s() * 1e6 ); - assert!(delta.rmag_km() < 2e-16, "Position error should be zero"); - assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); + assert!( + delta.rmag_km() < f64::EPSILON, + "Position error should be zero" + ); + assert!( + delta.vmag_km_s() < f64::EPSILON, + "Velocity error should be zero" + ); // Iterate odp.iterate_arc( @@ -823,8 +841,14 @@ fn od_tb_val_az_el_ckf_fixed_step_perfect_stations( delta.vmag_km_s() * 1e6 ); - assert!(delta.rmag_km() < 2e-16, "Position error should be zero"); - assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); + assert!( + delta.rmag_km() < f64::EPSILON, + "Position error should be zero" + ); + assert!( + delta.vmag_km_s() < f64::EPSILON, + "Velocity error should be zero" + ); // Iterate odp.iterate_arc( @@ -1311,8 +1335,14 @@ fn od_tb_val_harmonics_ckf_fixed_step_perfect_cov_test( delta.vmag_km_s() * 1e6 ); - assert!(delta.rmag_km() < 2e-16, "Position error should be zero"); - assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); + assert!( + delta.rmag_km() < f64::EPSILON, + "Position error should be zero" + ); + assert!( + delta.vmag_km_s() < f64::EPSILON, + "Velocity error should be zero" + ); } #[allow(clippy::identity_op)] @@ -1428,6 +1458,12 @@ fn od_tb_ckf_fixed_step_perfect_stations_several_snc_covar_map( delta.vmag_km_s() * 1e3 ); - assert!(delta.rmag_km() < 2e-16, "Position error should be zero"); - assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); + assert!( + delta.rmag_km() < f64::EPSILON, + "Position error should be zero" + ); + assert!( + delta.vmag_km_s() < f64::EPSILON, + "Velocity error should be zero" + ); } diff --git a/tests/propagation/stopcond.rs b/tests/propagation/stopcond.rs index a832cdfd..30ad79b1 100644 --- a/tests/propagation/stopcond.rs +++ b/tests/propagation/stopcond.rs @@ -10,7 +10,7 @@ use anise::prelude::Almanac; use hifitime::JD_J2000; use nalgebra::Vector3; use nyx::cosmic::Orbit; -use nyx::dynamics::guidance::{FiniteBurns, LocalFrame, Mnvr, Thruster}; +use nyx::dynamics::guidance::{FiniteBurns, LocalFrame, Maneuver, Thruster}; use nyx::dynamics::orbital::OrbitalDynamics; use nyx::dynamics::SpacecraftDynamics; use nyx::md::{Event, StateParameter}; @@ -313,7 +313,7 @@ fn event_and_combination(almanac: Arc) { ); // Thrust in the +X direction continuously - let burn = FiniteBurns::from_mnvrs(vec![Mnvr::from_time_invariant( + let burn = FiniteBurns::from_mnvrs(vec![Maneuver::from_time_invariant( epoch + 1.minutes(), epoch + 15.minutes(), 1.0, @@ -331,15 +331,15 @@ fn event_and_combination(almanac: Arc) { .until_event(orbit.period().unwrap() * 4.0, &Event::apoapsis()) .unwrap(); - // Check that the fuel always decreases or stays constant - let mut cur_fuel = traj.states[0].fuel_mass_kg; + // Check that the prop always decreases or stays constant + let mut cur_prop = traj.states[0].mass.prop_mass_kg; for state in traj.states.iter().skip(1) { assert!( - state.fuel_mass_kg - cur_fuel <= 1e-6, // Check that fuel never increases, at least a mg level - "{cur_fuel} > {}", - state.fuel_mass_kg + state.mass.prop_mass_kg - cur_prop <= 1e-6, // Check that prop never increases, at least a mg level + "{cur_prop} > {}", + state.mass.prop_mass_kg ); - cur_fuel = state.fuel_mass_kg; + cur_prop = state.mass.prop_mass_kg; } // Convert the trajectory to the Moon frame diff --git a/tests/propagation/trajectory.rs b/tests/propagation/trajectory.rs index b5a95b03..b7b22f04 100644 --- a/tests/propagation/trajectory.rs +++ b/tests/propagation/trajectory.rs @@ -7,12 +7,12 @@ use nyx::cosmic::eclipse::EclipseLocator; use nyx::cosmic::{GuidanceMode, Orbit, Spacecraft}; use nyx::dynamics::guidance::{GuidanceLaw, Ruggiero, Thruster}; use nyx::dynamics::{OrbitalDynamics, SpacecraftDynamics}; -use nyx::io::trajectory_data::TrajectoryLoader; use nyx::md::prelude::{ExportCfg, Objective}; use nyx::md::StateParameter; use nyx::propagators::*; use nyx::time::{Epoch, TimeSeries, Unit}; use nyx::State; +use nyx_space::md::Trajectory; use std::path::PathBuf; use std::sync::mpsc::channel; use std::sync::Arc; @@ -28,7 +28,7 @@ fn almanac() -> Arc { #[allow(clippy::identity_op)] #[rstest] -fn traj_ephem_forward(almanac: Arc) { +fn traj_ephem_forward_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); // Test that we can correctly interpolate a spacecraft orbit @@ -143,8 +143,7 @@ fn traj_ephem_forward(almanac: Arc) { // Reload this trajectory and make sure that it matches - let dyn_traj = TrajectoryLoader::from_parquet(exported_path).unwrap(); - let concrete_traj = dyn_traj.to_traj::().unwrap(); + let concrete_traj = Trajectory::from_parquet(exported_path).unwrap(); if ephem != concrete_traj { // Uh oh, let's see where the differences are. @@ -252,7 +251,7 @@ fn traj_ephem_forward(almanac: Arc) { #[rstest] fn traj_spacecraft(almanac: Arc) { let _ = pretty_env_logger::try_init(); - // Test the interpolation of a spaceraft trajectory and of its fuel. Includes a demo of checking what the guidance mode _should_ be provided the state. + // Test the interpolation of a spaceraft trajectory and of its prop. Includes a demo of checking what the guidance mode _should_ be provided the state. // Note that we _do not_ attempt to interpolate the Guidance Mode. // This is based on the Ruggiero AOP correction @@ -273,7 +272,7 @@ fn traj_spacecraft(almanac: Arc) { let ruggiero_ctrl = Ruggiero::simple(objectives, orbit.into()).unwrap(); // Build the spacecraft state - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; // Define the thruster let lowt = Thruster { @@ -281,7 +280,7 @@ fn traj_spacecraft(almanac: Arc) { isp_s: 1650.0, }; let start_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc_dynamics = SpacecraftDynamics::from_guidance_law(OrbitalDynamics::two_body(), ruggiero_ctrl.clone()); @@ -354,7 +353,7 @@ fn traj_spacecraft(almanac: Arc) { let mut max_pos_err = (eval_state.orbit.radius_km - start_state.orbit.radius_km).norm(); let mut max_vel_err = (eval_state.orbit.velocity_km_s - start_state.orbit.velocity_km_s).norm(); - let mut max_fuel_err = eval_state.fuel_mass_kg - start_state.fuel_mass_kg; + let mut max_prop_err = eval_state.mass.prop_mass_kg - start_state.mass.prop_mass_kg; let mut max_err = (eval_state.to_vector() - start_state.to_vector()).norm(); while let Ok(prop_state) = rx.recv() { @@ -374,12 +373,12 @@ fn traj_spacecraft(almanac: Arc) { prop_state.epoch() ); } - let fuel_err = eval_state.fuel_mass_kg - prop_state.fuel_mass_kg; - if fuel_err > max_fuel_err { - max_fuel_err = fuel_err; + let prop_err = eval_state.mass.prop_mass_kg - prop_state.mass.prop_mass_kg; + if prop_err > max_prop_err { + max_prop_err = prop_err; println!( - "fuel_err = {:.3e} g @ {}", - fuel_err * 1e3, + "prop_err = {:.3e} g @ {}", + prop_err * 1e3, prop_state.epoch() ); } @@ -390,10 +389,10 @@ fn traj_spacecraft(almanac: Arc) { } println!( - "[traj_spacecraft] Maximum interpolation error: pos: {:.2e} m\t\tvel: {:.2e} m/s\t\tfuel: {:.2e} g\t\tfull state: {:.2e} (no unit)", + "[traj_spacecraft] Maximum interpolation error: pos: {:.2e} m\t\tvel: {:.2e} m/s\t\tprop: {:.2e} g\t\tfull state: {:.2e} (no unit)", max_pos_err * 1e3, max_vel_err * 1e3, - max_fuel_err * 1e3, + max_prop_err * 1e3, max_err ); @@ -412,7 +411,7 @@ fn traj_spacecraft(almanac: Arc) { // Allow for up to 0.1 gram error assert!( max_vel_err < 1e-4, - "Maximum spacecraft fuel in interpolation is too high!" + "Maximum spacecraft prop in interpolation is too high!" ); // And let's convert into another frame and back to check the error @@ -428,7 +427,7 @@ fn traj_spacecraft(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn traj_ephem_backward_cov_test(almanac: Arc) { +fn traj_ephem_backward(almanac: Arc) { // Test that we can correctly interpolate a spacecraft orbit let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/propulsion/closedloop_multi_oe_ruggiero.rs b/tests/propulsion/closedloop_multi_oe_ruggiero.rs index 88e6c190..e6010da0 100644 --- a/tests/propulsion/closedloop_multi_oe_ruggiero.rs +++ b/tests/propulsion/closedloop_multi_oe_ruggiero.rs @@ -59,10 +59,10 @@ fn qlaw_as_ruggiero_case_a(almanac: Arc) { let ruggiero_ctrl = Ruggiero::simple(objectives, orbit.into()).unwrap(); let dry_mass = 1.0; - let fuel_mass = 299.0; + let prop_mass = 299.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, ruggiero_ctrl); println!("[qlaw_as_ruggiero_case_a] {:x}", orbit); @@ -74,9 +74,9 @@ fn qlaw_as_ruggiero_case_a(almanac: Arc) { ); let mut prop = setup.with(sc_state, almanac.clone()); let (final_state, traj) = prop.for_duration_with_traj(prop_time).unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[qlaw_as_ruggiero_case_a] {:x}", final_state.orbit); - println!("[qlaw_as_ruggiero_case_a] fuel usage: {:.3} kg", fuel_usage); + println!("[qlaw_as_ruggiero_case_a] prop usage: {:.3} kg", prop_usage); // Find all of the events for e in &events { println!( @@ -91,7 +91,7 @@ fn qlaw_as_ruggiero_case_a(almanac: Arc) { "objective not achieved" ); - assert!((fuel_usage - 93.449).abs() < 1.0); + assert!((prop_usage - 93.449).abs() < 1.0); } #[rstest] @@ -123,11 +123,11 @@ fn qlaw_as_ruggiero_case_b(almanac: Arc) { let ruggiero_ctrl = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 1999.9; + let prop_mass = 1999.9; let dry_mass = 0.1; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, ruggiero_ctrl); println!("[qlaw_as_ruggiero_case_b] {:x}", orbit); @@ -141,16 +141,16 @@ fn qlaw_as_ruggiero_case_b(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[qlaw_as_ruggiero_case_b] {:x}", final_state.orbit); - println!("[qlaw_as_ruggiero_case_b] fuel usage: {:.3} kg", fuel_usage); + println!("[qlaw_as_ruggiero_case_b] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 223.515).abs() < 1.0); + assert!((prop_usage - 223.515).abs() < 1.0); } #[rstest] @@ -181,11 +181,11 @@ fn qlaw_as_ruggiero_case_c_cov_test(almanac: Arc) { let ruggiero_ctrl = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 299.9; + let prop_mass = 299.9; let dry_mass = 0.1; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, ruggiero_ctrl); println!("[qlaw_as_ruggiero_case_c] {:x}", orbit); @@ -199,15 +199,15 @@ fn qlaw_as_ruggiero_case_c_cov_test(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[qlaw_as_ruggiero_case_c] {:x}", final_state.orbit); - println!("[qlaw_as_ruggiero_case_c] fuel usage: {:.3} kg", fuel_usage); + println!("[qlaw_as_ruggiero_case_c] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 41.742).abs() < 1.0); + assert!((prop_usage - 41.742).abs() < 1.0); } #[rstest] @@ -242,11 +242,11 @@ fn qlaw_as_ruggiero_case_d(almanac: Arc) { let ruggiero_ctrl = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, ruggiero_ctrl); println!("[qlaw_as_ruggiero_case_d] {:x}", orbit); @@ -260,16 +260,16 @@ fn qlaw_as_ruggiero_case_d(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[qlaw_as_ruggiero_case_d] {:x}", final_state.orbit); - println!("[qlaw_as_ruggiero_case_d] fuel usage: {:.3} kg", fuel_usage); + println!("[qlaw_as_ruggiero_case_d] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 23.0).abs() < 1.0); + assert!((prop_usage - 23.0).abs() < 1.0); } #[rstest] @@ -305,11 +305,11 @@ fn qlaw_as_ruggiero_case_e(almanac: Arc) { let ruggiero_ctrl = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 1999.9; + let prop_mass = 1999.9; let dry_mass = 0.1; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, ruggiero_ctrl); println!("[qlaw_as_ruggiero_case_e] {:x}", orbit); @@ -323,16 +323,16 @@ fn qlaw_as_ruggiero_case_e(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[qlaw_as_ruggiero_case_e] {:x}", final_state.orbit); - println!("[qlaw_as_ruggiero_case_e] fuel usage: {:.3} kg", fuel_usage); + println!("[qlaw_as_ruggiero_case_e] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 23.0).abs() < 1.0); + assert!((prop_usage - 23.0).abs() < 1.0); } #[rstest] @@ -364,11 +364,11 @@ fn qlaw_as_ruggiero_case_f(almanac: Arc) { let ruggiero_ctrl = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, ruggiero_ctrl); println!("[qlaw_as_ruggiero_case_f] {:x}", orbit); @@ -387,16 +387,16 @@ fn qlaw_as_ruggiero_case_f(almanac: Arc) { traj.to_parquet_simple("output_data/rugg_case_f.parquet", almanac) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[qlaw_as_ruggiero_case_f] {:x}", final_state.orbit); - println!("[qlaw_as_ruggiero_case_f] fuel usage: {:.3} kg", fuel_usage); + println!("[qlaw_as_ruggiero_case_f] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 10.378).abs() < 1.0); + assert!((prop_usage - 10.378).abs() < 1.0); } #[rstest] @@ -427,11 +427,11 @@ fn ruggiero_iepc_2011_102(almanac: Arc) { let ruggiero_ctrl = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, ruggiero_ctrl); println!("[ruggiero_iepc_2011_102] {:x}", orbit); @@ -445,15 +445,15 @@ fn ruggiero_iepc_2011_102(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[ruggiero_iepc_2011_102] {:x}", final_state.orbit); - println!("[ruggiero_iepc_2011_102] fuel usage: {:.3} kg", fuel_usage); + println!("[ruggiero_iepc_2011_102] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - // WARNING: Paper claims this can be done with only 49kg of fuel. - assert!((fuel_usage - 49.0).abs() < 1.0); + // WARNING: Paper claims this can be done with only 49kg of prop. + assert!((prop_usage - 49.0).abs() < 1.0); } diff --git a/tests/propulsion/closedloop_single_oe_ruggiero.rs b/tests/propulsion/closedloop_single_oe_ruggiero.rs index ca3b3bf3..0a511cb3 100644 --- a/tests/propulsion/closedloop_single_oe_ruggiero.rs +++ b/tests/propulsion/closedloop_single_oe_ruggiero.rs @@ -46,10 +46,10 @@ fn rugg_sma(almanac: Arc) { let guid_law = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, guid_law); println!("[rugg_sma] {:x}", orbit); @@ -63,15 +63,15 @@ fn rugg_sma(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_sma] {:x}", final_state.orbit); - println!("[rugg_sma] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_sma] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 21.0).abs() < 1.0); + assert!((prop_usage - 21.0).abs() < 1.0); } #[rstest] @@ -97,12 +97,12 @@ fn rugg_sma_regress_threshold(almanac: Arc) { 1.0, )]; - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; - for (threshold, expected_fuel_usage) in &[(0.9, 16.9), (0.0, 21.3)] { + for (threshold, expected_prop_usage) in &[(0.9, 16.9), (0.0, 21.3)] { let guid_law = Ruggiero::from_ηthresholds(objectives, &[*threshold], orbit.into()).unwrap(); let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(OrbitalDynamics::two_body(), guid_law); println!("[rugg_sma_regress] {:x}", orbit); @@ -116,15 +116,15 @@ fn rugg_sma_regress_threshold(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_sma_regress] {:x}", final_state.orbit); - println!("[rugg_sma_regress] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_sma_regress] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - *expected_fuel_usage).abs() < 0.5); + assert!((prop_usage - *expected_prop_usage).abs() < 0.5); } } @@ -156,10 +156,10 @@ fn rugg_sma_decr(almanac: Arc) { let guid_law = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, guid_law); println!("[rugg_sma_decr] {:x}", orbit); @@ -173,15 +173,15 @@ fn rugg_sma_decr(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_sma_decr] {:x}", final_state.orbit); - println!("[rugg_sma_decr] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_sma_decr] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 21.0).abs() < 1.0); + assert!((prop_usage - 21.0).abs() < 1.0); } #[rstest] @@ -214,10 +214,10 @@ fn rugg_inc(almanac: Arc) { let guid_law = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, guid_law); println!("[rugg_inc] {:x}", orbit); @@ -231,15 +231,15 @@ fn rugg_inc(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_inc] {:x}", final_state.orbit); - println!("[rugg_inc] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_inc] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 25.0).abs() < 1.0); + assert!((prop_usage - 25.0).abs() < 1.0); } #[rstest] @@ -273,10 +273,10 @@ fn rugg_inc_threshold(almanac: Arc) { let guid_law = Ruggiero::from_ηthresholds(objectives, &[0.9], orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, guid_law); println!("[rugg_inc_threshold] {:x}", orbit); @@ -290,15 +290,15 @@ fn rugg_inc_threshold(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_inc_threshold] {:x}", final_state.orbit); - println!("[rugg_inc_threshold] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_inc_threshold] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 17.0).abs() < 1.0); + assert!((prop_usage - 17.0).abs() < 1.0); } #[rstest] @@ -331,10 +331,10 @@ fn rugg_inc_decr(almanac: Arc) { let guid_law = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, guid_law); println!("[rugg_inc_decr] {:x}", orbit); @@ -348,15 +348,15 @@ fn rugg_inc_decr(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_inc_decr] {:x}", final_state.orbit); - println!("[rugg_inc_decr] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_inc_decr] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 25.0).abs() < 1.0); + assert!((prop_usage - 25.0).abs() < 1.0); } #[rstest] @@ -389,10 +389,10 @@ fn rugg_ecc(almanac: Arc) { let guid_law = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, guid_law); println!("[rugg_ecc] {:x}", orbit); @@ -406,15 +406,15 @@ fn rugg_ecc(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_ecc] {:x}", final_state.orbit); - println!("[rugg_ecc] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_ecc] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 10.37).abs() < 1.0); + assert!((prop_usage - 10.37).abs() < 1.0); } #[rstest] @@ -429,7 +429,7 @@ fn rugg_ecc_regress_threshold(almanac: Arc) { let prop_time = 150 * Unit::Day; - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; // Define the thruster @@ -445,11 +445,11 @@ fn rugg_ecc_regress_threshold(almanac: Arc) { 5e-5, )]; - for (threshold, expected_fuel_usage) in &[(0.9, 8.2), (0.0, 10.37)] { + for (threshold, expected_prop_usage) in &[(0.9, 8.2), (0.0, 10.37)] { let guid_law = Ruggiero::from_ηthresholds(objectives, &[*threshold], orbit.into()).unwrap(); let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(OrbitalDynamics::two_body(), guid_law); println!("[rugg_ecc_regress] {:x}", orbit); @@ -463,15 +463,15 @@ fn rugg_ecc_regress_threshold(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_ecc_regress] {:x}", final_state.orbit); - println!("[rugg_ecc_regress] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_ecc_regress] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - *expected_fuel_usage).abs() < 1.0); + assert!((prop_usage - *expected_prop_usage).abs() < 1.0); } } @@ -505,10 +505,10 @@ fn rugg_ecc_decr(almanac: Arc) { let guid_law = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, guid_law); println!("[rugg_ecc_decr] {:x}", orbit); @@ -522,15 +522,15 @@ fn rugg_ecc_decr(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_ecc_decr] {:x}", final_state.orbit); - println!("[rugg_ecc_decr] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_ecc_decr] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 10.37).abs() < 1.0); + assert!((prop_usage - 10.37).abs() < 1.0); } #[rstest] @@ -565,10 +565,10 @@ fn rugg_aop(almanac: Arc) { let guid_law = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, guid_law); println!("[rugg_aop] {:x}", orbit); @@ -582,15 +582,15 @@ fn rugg_aop(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_aop] {:x}", final_state.orbit); - println!("[rugg_aop] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_aop] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 0.014).abs() < 1e-2); + assert!((prop_usage - 0.014).abs() < 1e-2); } #[rstest] @@ -624,10 +624,10 @@ fn rugg_aop_decr(almanac: Arc) { let guid_law = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, guid_law); println!("[rugg_aop_decr] {:x}", orbit); @@ -641,15 +641,15 @@ fn rugg_aop_decr(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_aop_decr] {:x}", final_state.orbit); - println!("[rugg_aop_decr] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_aop_decr] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 0.014).abs() < 1e-2); + assert!((prop_usage - 0.014).abs() < 1e-2); } #[rstest] @@ -680,10 +680,10 @@ fn rugg_raan(almanac: Arc) { let guid_law = Ruggiero::simple(objectives, orbit.into()).unwrap(); - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(orbital_dyn, guid_law); println!("[rugg_raan] {:x}", orbit); @@ -695,17 +695,17 @@ fn rugg_raan(almanac: Arc) { ); let mut prop = setup.with(sc_state, almanac.clone()); let (final_state, traj) = prop.for_duration_with_traj(prop_time).unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_raan] {:x}", final_state.orbit); let event = Event::new(StateParameter::RAAN, 5.0); println!("[rugg_raan] {} => {:?}", event, traj.find(&event, almanac)); - println!("[rugg_raan] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_raan] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - 22.189).abs() < 1.0); + assert!((prop_usage - 22.189).abs() < 1.0); } #[rstest] @@ -720,7 +720,7 @@ fn rugg_raan_regress_threshold(almanac: Arc) { let prop_time = 130 * Unit::Day; - let fuel_mass = 67.0; + let prop_mass = 67.0; let dry_mass = 300.0; // Define the thruster @@ -732,11 +732,11 @@ fn rugg_raan_regress_threshold(almanac: Arc) { // Define the objectives let objectives = &[Objective::within_tolerance(StateParameter::RAAN, 5.0, 5e-5)]; - for (threshold, expected_fuel_usage) in &[(0.9, 14.787), (0.0, 22.189)] { + for (threshold, expected_prop_usage) in &[(0.9, 14.787), (0.0, 22.189)] { let guid_law = Ruggiero::from_ηthresholds(objectives, &[*threshold], orbit.into()).unwrap(); let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, lowt, GuidanceMode::Thrust); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, lowt, GuidanceMode::Thrust); let sc = SpacecraftDynamics::from_guidance_law(OrbitalDynamics::two_body(), guid_law); println!("[rugg_raan_regress] {:x}", orbit); @@ -750,14 +750,14 @@ fn rugg_raan_regress_threshold(almanac: Arc) { .for_duration(prop_time) .unwrap(); - let fuel_usage = fuel_mass - final_state.fuel_mass_kg; + let prop_usage = prop_mass - final_state.mass.prop_mass_kg; println!("[rugg_raan_regress] {:x}", final_state.orbit); - println!("[rugg_raan_regress] fuel usage: {:.3} kg", fuel_usage); + println!("[rugg_raan_regress] prop usage: {:.3} kg", prop_usage); assert!( sc.guidance_achieved(&final_state).unwrap(), "objective not achieved" ); - assert!((fuel_usage - *expected_fuel_usage).abs() < 1e-1); + assert!((prop_usage - *expected_prop_usage).abs() < 1e-1); } } diff --git a/tests/propulsion/schedule.rs b/tests/propulsion/schedule.rs index b276917c..011a153a 100644 --- a/tests/propulsion/schedule.rs +++ b/tests/propulsion/schedule.rs @@ -1,6 +1,6 @@ extern crate nyx_space as nyx; use self::nyx::cosmic::{GuidanceMode, Orbit, Spacecraft}; -use self::nyx::dynamics::guidance::{FiniteBurns, Mnvr, Thruster}; +use self::nyx::dynamics::guidance::{FiniteBurns, Maneuver, Thruster}; use self::nyx::dynamics::{OrbitalDynamics, SpacecraftDynamics}; use self::nyx::linalg::Vector3; use self::nyx::propagators::{IntegratorOptions, Propagator}; @@ -44,9 +44,9 @@ fn val_transfer_schedule_no_depl(almanac: Arc) { isp_s: 300.0, }; let dry_mass = 1e3; - let fuel_mass = 756.0; + let prop_mass = 756.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, monoprop, GuidanceMode::Coast); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, monoprop, GuidanceMode::Coast); let prop_time = 50.0 * Unit::Minute; @@ -57,7 +57,7 @@ fn val_transfer_schedule_no_depl(almanac: Arc) { let orbital_dyn = OrbitalDynamics::point_masses(bodies); // Define the maneuver and its schedule - let mnvr0 = Mnvr::from_time_invariant( + let mnvr0 = Maneuver::from_time_invariant( Epoch::from_gregorian_tai_at_midnight(2002, 1, 1), end_time, 1.0, // Full thrust @@ -68,7 +68,7 @@ fn val_transfer_schedule_no_depl(almanac: Arc) { let schedule = FiniteBurns::from_mnvrs(vec![mnvr0]); // And create the spacecraft with that controller - // Disable fuel mass decrement + // Disable prop mass decrement let sc = SpacecraftDynamics::from_guidance_law_no_decr(orbital_dyn, schedule); // Setup a propagator, and propagate for that duration // NOTE: We specify the use an RK89 to match the GMAT setup. @@ -108,10 +108,10 @@ fn val_transfer_schedule_no_depl(almanac: Arc) { assert!(err_r < 5e-10, "finite burn position wrong: {:.5e}", err_r); assert!(err_v < 6e-13, "finite burn velocity wrong: {:.5e}", err_v); - // Ensure that there was no change in fuel mass since tank depletion was off + // Ensure that there was no change in prop mass since tank depletion was off assert!( - (final_state.fuel_mass_kg - fuel_mass).abs() < f64::EPSILON, - "incorrect fuel mass" + (final_state.mass.prop_mass_kg - prop_mass).abs() < f64::EPSILON, + "incorrect prop mass" ); } @@ -134,9 +134,9 @@ fn val_transfer_schedule_depl_cov_test(almanac: Arc) { isp_s: 300.0, }; let dry_mass = 1e3; - let fuel_mass = 756.0; + let prop_mass = 756.0; let sc_state = - Spacecraft::from_thruster(orbit, dry_mass, fuel_mass, monoprop, GuidanceMode::Coast); + Spacecraft::from_thruster(orbit, dry_mass, prop_mass, monoprop, GuidanceMode::Coast); let prop_time = 50.0 * Unit::Minute; @@ -149,7 +149,7 @@ fn val_transfer_schedule_depl_cov_test(almanac: Arc) { // With 100% thrust: RSS errors: pos = 3.14651e1 km vel = 3.75245e-2 km/s // Define the maneuver and its schedule - let mnvr0 = Mnvr::from_time_invariant( + let mnvr0 = Maneuver::from_time_invariant( Epoch::from_gregorian_tai_at_midnight(2002, 1, 1), end_time, 1.0, // Full thrust @@ -181,7 +181,7 @@ fn val_transfer_schedule_depl_cov_test(almanac: Arc) { eme2k, ); - let rslt_fuel_mass = 745.802_837_870_161; + let rslt_prop_mass = 745.802_837_870_161; let (err_r, err_v) = rss_orbit_vec_errors( &final_state.orbit.to_cartesian_pos_vel(), @@ -202,9 +202,9 @@ fn val_transfer_schedule_depl_cov_test(almanac: Arc) { assert!(err_r < 5e-10, "finite burn position wrong: {:.5e}", err_r); assert!(err_v < 5e-13, "finite burn velocity wrong: {:.5e}", err_v); - let delta_fuel_mass = (final_state.fuel_mass_kg - rslt_fuel_mass).abs(); - println!("Absolute fuel mass error: {:.0e} kg", delta_fuel_mass); - assert!(delta_fuel_mass < 2e-10, "incorrect fuel mass"); + let delta_prop_mass = (final_state.mass.prop_mass_kg - rslt_prop_mass).abs(); + println!("Absolute prop mass error: {:.0e} kg", delta_prop_mass); + assert!(delta_prop_mass < 2e-10, "incorrect prop mass"); // Now, test that backward propagation of maneuvers also works. let backward_state = setup @@ -262,11 +262,11 @@ fn val_transfer_single_maneuver_depl_cov_test(almanac: Arc) { isp_s: 300.0, }; let dry_mass_kg = 1e3; - let fuel_mass_kg = 756.0; + let prop_mass_kg = 756.0; let sc_state = Spacecraft::from_thruster( orbit, dry_mass_kg, - fuel_mass_kg, + prop_mass_kg, monoprop, GuidanceMode::Coast, ); @@ -282,7 +282,7 @@ fn val_transfer_single_maneuver_depl_cov_test(almanac: Arc) { // With 100% thrust: RSS errors: pos = 3.14651e1 km vel = 3.75245e-2 km/s // Define the maneuver and its schedule - let mnvr0 = Mnvr::from_time_invariant( + let mnvr0 = Maneuver::from_time_invariant( Epoch::from_gregorian_tai_at_midnight(2002, 1, 1), end_time, 1.0, // Full thrust @@ -312,7 +312,7 @@ fn val_transfer_single_maneuver_depl_cov_test(almanac: Arc) { eme2k, ); - let rslt_fuel_mass = 745.802_837_870_161; + let rslt_prop_mass = 745.802_837_870_161; let (err_r, err_v) = rss_orbit_vec_errors( &final_state.orbit.to_cartesian_pos_vel(), @@ -333,9 +333,9 @@ fn val_transfer_single_maneuver_depl_cov_test(almanac: Arc) { assert!(err_r < 5e-10, "finite burn position wrong: {:.5e}", err_r); assert!(err_v < 5e-13, "finite burn velocity wrong: {:.5e}", err_v); - let delta_fuel_mass = (final_state.fuel_mass_kg - rslt_fuel_mass).abs(); - println!("Absolute fuel mass error: {:.0e} kg", delta_fuel_mass); - assert!(delta_fuel_mass < 2e-10, "incorrect fuel mass"); + let delta_prop_mass = (final_state.mass.prop_mass_kg - rslt_prop_mass).abs(); + println!("Absolute prop mass error: {:.0e} kg", delta_prop_mass); + assert!(delta_prop_mass < 2e-10, "incorrect prop mass"); // Now, test that backward propagation of maneuvers also works. let backward_state = setup diff --git a/tests/python/.gitignore b/tests/python/.gitignore deleted file mode 100644 index 52a8c3d9..00000000 --- a/tests/python/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -__pycache__ -private -mod.rs \ No newline at end of file diff --git a/tests/python/README.md b/tests/python/README.md deleted file mode 100644 index 8ce995c2..00000000 --- a/tests/python/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# Python tests - -The Python tests only tests the functionality and usability. All other tests are done in Rust with the `cargo test` command. This is why the Python tests are not exhaustive. \ No newline at end of file diff --git a/tests/python/test_cosmic.py b/tests/python/test_cosmic.py deleted file mode 100644 index c5ce7d9f..00000000 --- a/tests/python/test_cosmic.py +++ /dev/null @@ -1,167 +0,0 @@ -from nyx_space.cosmic import Orbit, Spacecraft, SrpConfig, DragConfig -from nyx_space.time import Epoch, Unit, Duration -from nyx_space.monte_carlo import generate_orbits, generate_spacecraft, StateParameter -import pickle - - -def test_load_cosm(): - cosm = Cosm.de438() - assert cosm.frames_get_names() == [ - "SSB J2000", - "Sun J2000", - "iau sun", - "Mercury Barycenter J2000", - "Venus Barycenter J2000", - "iau venus", - "Earth Barycenter J2000", - "Earth J2000", - "iau earth", - "Moon J2000", - "iau moon", - "Mars Barycenter J2000", - "iau mars", - "Jupiter Barycenter J2000", - "iau jupiter", - "Saturn Barycenter J2000", - "iau saturn", - "Uranus Barycenter J2000", - "iau uranus", - "Neptune Barycenter J2000", - "iau neptune", - "Pluto Barycenter J2000", - ] - - -def test_define_spacecraft(): - cosm = Cosm.de438() - eme2k = cosm.frame("EME2000") - - assert eme2k.gm() == 398600.435392 # SPICE data, not GMAT data (398600.4415) - assert eme2k.is_geoid() - assert eme2k.equatorial_radius() == 6378.1363 - - e = Epoch.system_now() - - orbit = Orbit.from_try_keplerian_altitude( - 400, - ecc=1e-4, - inc_deg=30.5, - raan_deg=35.0, - aop_deg=65.0, - ta_deg=590, - epoch=e, - frame=eme2k, - ) - - assert orbit.period().unwrap() == Unit.Second * 5553.623455582 - - print(orbit) - print(repr(orbit)) - - # Define the SRP - srp = SrpConfig(2.0) - # Define the Drag - drag = DragConfig(2.0) - - # Build a spacecraft - sc = Spacecraft( - orbit, - dry_mass_kg=500.0, - fuel_mass_kg=15.0, - srp=srp, - drag=drag, - ) - - print(sc) - - print(sc.orbit) - - # Check that we can pickle and dump the config of this spacecraft - pkld = pickle.dumps(sc) - print(sc.dumps()) - sc_loaded = Spacecraft.loads(sc.dumps()) - assert sc_loaded == sc - unpkld = pickle.loads(pkld) - assert unpkld == sc - - # NOTE: This does not return a pointer to the object, but a new object! - # Therefore the equality _will_ fail! - assert orbit == sc.orbit - # But the printed information is identical - assert f"{orbit}" == f"{sc.orbit}" - # And the epochs match - assert e.timedelta(sc.epoch) == Duration.zero() - - -def test_generate_states(): - """ - Tests that we can generate orbits from their state parameter deviations - """ - # Build a demo orbit - cosm = Cosm.de438() - eme2k = cosm.frame("EME2000") - - assert eme2k.gm() == 398600.435392 # SPICE data, not GMAT data (398600.4415) - assert eme2k.is_geoid() - assert eme2k.equatorial_radius() == 6378.1363 - - e = Epoch.system_now() - - orbit = Orbit.from_try_keplerian_altitude( - 400, - ecc=1e-4, - inc_deg=30.5, - raan_deg=35.0, - aop_deg=65.0, - ta_deg=590, - epoch=e, - frame=eme2k, - ) - - # Generate a bunch of them in percentage of error - orbits = generate_orbits( - orbit, - [ - # Note that we can create the state parameter from a string of its name - (StateParameter("SMA"), 0.05), - # Or directly as an enum (the preferred method) - (StateParameter.Eccentricity, 0.1), - (StateParameter.Inclination, 0.1), - ], - 100, - kind="prct", - ) - assert len(orbits) >= 98 - - # Define the SRP - srp = SrpConfig(2.0) - # Define the Drag - drag = DragConfig(2.0) - - # Build a spacecraft - sc = Spacecraft( - orbit, - dry_mass_kg=500.0, - fuel_mass_kg=15.0, - srp=srp, - drag=drag, - ) - - # Generate a bunch of spacecraft with a specific seed, and using the absolute errors - spacecraft = generate_spacecraft( - sc, - [ - (StateParameter.FuelMass, 1.9), - (StateParameter.SMA, 1.0), - (StateParameter.Eccentricity, 0.01), - (StateParameter.Inclination, 3.5), - ], - 100, - kind="abs", - seed=42, - ) - assert len(spacecraft) >= 98 - - -if __name__ == "__main__": - test_define_spacecraft() diff --git a/tests/python/test_gauss_markov.py b/tests/python/test_gauss_markov.py deleted file mode 100644 index eee32c7e..00000000 --- a/tests/python/test_gauss_markov.py +++ /dev/null @@ -1,81 +0,0 @@ -from nyx_space.orbit_determination import GaussMarkov -from nyx_space.time import Unit -from nyx_space.plots import plot_gauss_markov -from pathlib import Path -import pandas as pd - - -def test_fogm(plot=False): - """ - Tests a first order Gauss Markov process - """ - - # Base path - root = Path(__file__).joinpath("../../../").resolve() - outpath = root.joinpath("output_data/") - - gm = GaussMarkov(tau=Unit.Hour * 24, sigma=5.6, steady_state=0.5) - print(gm) - assert str(gm) == "First order Gauss-Markov process with τ = 1 days, σ_b = 5.6, σ_q = 0.5" - gm.simulate(str(outpath.joinpath("fogm.parquet"))) - - # Read in the file - df = pd.read_parquet(str(outpath.joinpath("fogm.parquet"))) - - # The seeds are chosen from entropy each time, - # so the bias should be non-zero but we can't predict what it will be. - assert df["Bias (unitless)"].mean() != 0.0 - assert df["Bias (unitless)"].max() != 0.0 - assert df["Bias (unitless)"].min() != 0.0 - assert df["Bias (unitless)"].count() == 12525 - - if plot: - plot_gauss_markov(df, title=f"{gm}", tau=gm.tau.to_seconds()) - - -def test_defaults(kinds=["Range", "RangeHP", "Doppler", "DopplerHP"], plot=False): - """ - Tests the four default models - """ - - # Base path - root = Path(__file__).joinpath("../../../").resolve() - outpath = root.joinpath("output_data/") - - for kind in kinds: - unit = "km" if "Range" in kind else "km/s" - gm = GaussMarkov.default(kind) - gm.simulate(str(outpath.joinpath(f"{kind}.parquet")), unit=unit) - - # Read in the file - df = pd.read_parquet(str(outpath.joinpath(f"{kind}.parquet"))) - - if plot: - plot_gauss_markov(df, title=f"{kind} - {gm}", tau=gm.tau.to_seconds()) - - -def test_load(plot=False): - """ - Tests that we can load a Gauss Markov process from a file and simulate it. - """ - # Base path - root = Path(__file__).joinpath("../../../").resolve() - config = root.joinpath("data/tests/config/high-prec-network.yaml") - outpath = root.joinpath("output_data/") - - models = GaussMarkov.load_named(str(config)) - - for name, gm in models.items(): - gm.simulate(str(outpath.joinpath(f"{name}.parquet"))) - - # Read in the file - df = pd.read_parquet(str(outpath.joinpath(f"{name}.parquet"))) - - if plot: - plot_gauss_markov(df, title=f"{name} - {gm}", tau=gm.tau.to_seconds()) - - -if __name__ == "__main__": - test_fogm(True) - test_defaults(plot=True) - test_load(True) diff --git a/tests/python/test_mission_design.py b/tests/python/test_mission_design.py deleted file mode 100644 index fa72f1e9..00000000 --- a/tests/python/test_mission_design.py +++ /dev/null @@ -1,323 +0,0 @@ -import logging -import pickle -import sys -from pathlib import Path -from timeit import timeit - -import pandas as pd -import yaml -from nyx_space.cosmic import Orbit, Spacecraft, SrpConfig -from nyx_space.mission_design import ( - Event, - SpacecraftDynamics, - StateParameter, - TrajectoryLoader, - propagate, - two_body, -) -from nyx_space.monte_carlo import StateParameter, generate_orbits -from nyx_space.plots import plot_traj_errors -from nyx_space.time import Duration, Epoch, TimeSeries, Unit - - -def test_propagate(): - # Initialize logging - FORMAT = "%(levelname)s %(name)s %(filename)s:%(lineno)d\t%(message)s" - logging.basicConfig(format=FORMAT) - logging.getLogger().setLevel(logging.INFO) - - # Base path - root = Path(__file__).joinpath("../../../").resolve() - - config_path = root.joinpath("./data/tests/config/") - - sc = Spacecraft.load(str(config_path.joinpath("spacecraft.yaml"))) - # Check that we have loaded this correctly by checking the values from the YAML file - assert sc.value_of(StateParameter.X) == -9042.862234 - assert sc.value_of(StateParameter.Y) == 18536.333069 - assert sc.value_of(StateParameter.Z) == 6999.957069 - assert sc.value_of(StateParameter.VX) == -3.288789 - assert sc.value_of(StateParameter.VY) == -2.226285 - assert sc.value_of(StateParameter.VZ) == 1.646738 - assert sc.value_of(StateParameter.Cr) == 1.0 - assert sc.value_of(StateParameter.Cd) == 2.2 - assert sc.value_of(StateParameter.FuelMass) == 50.0 - assert sc.epoch.timedelta(Epoch("2018-09-15T00:15:53.098 UTC")) == Duration.zero() - # Check other stuff that is computed on request - assert sc.value_of(StateParameter.SMA) == 21999.99774705774 - - dynamics = SpacecraftDynamics.load_named(str(config_path.joinpath("dynamics.yaml"))) - # So far, there are no accessors on the dynamics, so we will check what they print out =( - assert ( - f"{dynamics['lofi']}" - == "Spacecraft dynamics (with guidance = false): No force models; Orbital dynamics: Point masses of Sun J2000, Earth J2000" - ) - assert ( - f"{dynamics['hifi']}" - == "Spacecraft dynamics (with guidance = false): SRP with φ = 1367 W/m^2 and eclipse light-source: Sun J2000, shadows casted by: Sun J2000, Moon J2000; Orbital dynamics: Point masses of Sun J2000, Earth J2000, Moon J2000; IAU Earth gravity field 10x10 (order x degree)" - ) - - rslt, traj = propagate(sc, dynamics["lofi"], Unit.Day * 5.159) - # Check that we propagated for the correct duration - assert rslt.epoch.timedelta(sc.epoch) == Duration("5 days 3 h 48 min 57 s 600 ms") - # We aren't in two body dynamics so the SMA has changed among other things. - assert rslt.value_of(StateParameter.SMA) == 22000.024381883788 - # Grab a specific epoch from the trajectory - sc_state = traj.at(Epoch("2018-09-16T00:16:53 TDB")) - # Check that we got the right epoch - assert sc_state.epoch.timedelta(Epoch("2018-09-16T00:16:53 TDB")) == Duration.zero() - # Save the file to parquet - traj.to_parquet("lofi.parquet") - - # We can also propagate with a different method - rslt, traj = propagate(sc, dynamics["lofi"], Unit.Day * 5.159, method="Dormand78") - assert rslt.epoch.timedelta(sc.epoch) == Duration("5 days 3 h 48 min 57 s 600 ms") - - # Let's now propagate the original spacecraft to its apoapsis, but let's search no more than 2 orbit periods - rslt_apo, _ = propagate( - sc, - dynamics["lofi"], - sc.orbit.period().unwrap() * 2, - event=Event(StateParameter.Apoapsis, 0.0), - ) - # Let's make sure that worked to within the default precision - assert abs(rslt_apo.value_of(StateParameter.TrueAnomaly) - 180.0) < 0.001 - - event = Event(StateParameter.Apoapsis, 0.0, value_precision=1e-6) - - # But we can also be even more precise - rslt_apo, traj = propagate( - sc, - dynamics["lofi"], - sc.orbit.period().unwrap() * 2, - event=event, - ) - assert abs(rslt_apo.orbit.ta_deg() - 180.0) <= 1e-6 - - # Resample the trajectory at fixed step size of 25 seconds - traj = traj.resample(Unit.Second * 25.0) - - # Rebuild the trajectory with specific epochs - ts = TimeSeries( - traj.first().epoch, - traj.last().epoch, - step=Duration("0.3 min 56 s 15 ns"), - inclusive=True, - ) - epochs = [epoch for epoch in ts] - rebuilt_traj = traj.rebuild(epochs[1:-1]) - assert rebuilt_traj.first().epoch == epochs[1] - assert rebuilt_traj.last().epoch == epochs[-2] - - # Export this trajectory with additional metadata and the events - # Base path - root = Path(__file__).joinpath("../../../").resolve() - outpath = root.joinpath("output_data/") - - # Note: Python interface only supports strings for paths, not Path objects. - traj.to_parquet( - str(outpath.joinpath("./lofi_with_events.parquet")), - metadata={"dynamics": str(dynamics["lofi"])}, - events=[event], - ) - - # Propagate until the lo-fi epoch to compare - _, traj_hifi = propagate( - sc, - dynamics["hifi"], - epoch=rslt_apo.epoch, - ) - - # Plot both trajectories in RIC frame - if sys.platform != "win32": - diff_path = str(outpath.joinpath("./lofi_hifi_ric_diff.parquet")) - traj.ric_diff_to_parquet(traj_hifi, diff_path) - - traj_diff_ric = pd.read_parquet(diff_path) - - plot_traj_errors( - traj_diff_ric, - "LoFi vs HiFi", - html_out=outpath.joinpath("./md_ric_lofi_hifi_diff.html"), - show=False, - ) - - plot_traj_errors( - traj_diff_ric, - "LoFi vs HiFi", - html_out=outpath.joinpath("./md_ric_lofi_hifi_diff_velocity.html"), - vertical=True, - velocity=True, - show=False, - ) - - # Resample the trajectory at fixed step size of 25 seconds - traj = traj.resample(Unit.Second * 25.0) - - # Also export this ground track - cosm = Cosm.de438() - iau_earth = cosm.frame("IAU Earth") - traj.to_parquet("iau_earth_lofi.parquet", groundtrack=iau_earth) - - # Let's also search for this event in the trajectory - for sc_at_event in traj.find(event): - print(sc_at_event) - assert abs(sc_at_event.value_of(StateParameter.TrueAnomaly) - 180.0) <= 1e-6 - - -def test_build_spacecraft(): - """ - Tests that we can build a spacecraft from scratch without an input file - """ - # Initialize logging - FORMAT = "%(levelname)s %(name)s %(filename)s:%(lineno)d\t%(message)s" - logging.basicConfig(format=FORMAT) - logging.getLogger().setLevel(logging.INFO) - - cosm = Cosm.de438() - eme2k = cosm.frame("EME2000") # Earth Mean Equator J2000 - orbit = Orbit.from_try_keplerian_altitude( - 400.0, 0.01, 15.6, 45.0, 90.0, 75.0, Epoch.system_now(), eme2k - ) - # Define the SRP - srp = SrpConfig(2.0) # 2.0 will be the area - print(srp) - assert srp.cr == 1.8 # Default value - assert srp.area_m2 == 2.0 - sc = Spacecraft(orbit, 150.0, 15.0, srp) - print(sc) - - assert sc.srp() == srp - assert sc.drag().area_m2 == 0.0 - assert sc.drag().cd == 2.2 # Default value, but the area is zero, so it doesn't have any effect - - # Using this spacecraft as a template, let's load an OEM file, convert it to Parquet, and ensure we can load it back in. - # The orbit data will be overwritten with data from the OEM file. - root = Path(__file__).joinpath("../../../").resolve() - - config_path = root.joinpath("./data/tests/ccsds/oem/LEO_10s.oem") - output_path = root.joinpath("./output_data/LEO_10s.parquet") - # Convert from OEM to Parquet will happen on load - traj = TrajectoryLoader(str(config_path), "oem", str(output_path), sc) - print(traj) - # Check that we can pickle the trajectory loader object - traj_pkl = pickle.dumps(traj) - traj_unpkl = pickle.loads(traj_pkl) - assert traj_unpkl == traj - # Check that we can convert this to a spacecraft trajectory - traj_sc = traj.to_spacecraft_traj() - traj_orbit = traj.to_orbit_traj() - traj_orbit_dc = traj_sc.downcast() - # Check that we can query it (will raise an exception if we can't, thereby failing the test) - ts = TimeSeries( - Epoch("2020-06-01T12:00:00.000000"), - Epoch("2020-06-01T13:00:00.000000"), - step=Unit.Minute * 17 + Unit.Second * 13.8, - inclusive=True, - ) - for epoch in ts: - orbit = traj_orbit.at(epoch) - dc_orbit = traj_orbit_dc.at(epoch) - sc_orbit = traj_sc.at(epoch).orbit - # Check params individually - assert orbit.epoch == sc_orbit.epoch - assert orbit.x_km == sc_orbit.x_km - assert orbit.vx_km_s == sc_orbit.vx_km_s - # Check the downcasted version - assert dc_orbit.x_km == sc_orbit.x_km - assert dc_orbit.vx_km_s == sc_orbit.vx_km_s - assert dc_orbit == sc_orbit - - -def test_two_body(): - # Build a demo orbit - cosm = Cosm.de438() - eme2k = cosm.frame("EME2000") - - assert eme2k.gm() == 398600.435392 # SPICE data, not GMAT data (398600.4415) - assert eme2k.is_geoid() - assert eme2k.equatorial_radius() == 6378.1363 - - e = Epoch.system_now() - - orbit = Orbit.from_try_keplerian_altitude( - 400, - ecc=1e-4, - inc_deg=30.5, - raan_deg=35.0, - aop_deg=65.0, - ta_deg=590, - epoch=e, - frame=eme2k, - ) - - orbits = generate_orbits( - orbit, - [ - (StateParameter("SMA"), 0.05), - (StateParameter.Eccentricity, 0.1), - (StateParameter.Inclination, 0.1), - ], - 1000, - kind="prct", - ) - - # And propagate in parallel using a single duration - proped_orbits = two_body(orbits, durations=[Unit.Day * 531.5]) - assert len(proped_orbits) >= len(orbits) - 2 - - # And propagate in parallel using many epochs - ts = TimeSeries(e, e + Unit.Day * 1000, step=Unit.Day * 1, inclusive=False) - epochs = [e for e in ts] - proped_orbits = two_body(orbits, new_epochs=epochs) - # Allow up to two to fail - assert len(proped_orbits) >= len(orbits) - 2 - - timing = timeit(lambda: two_body(orbits, new_epochs=epochs), number=1) - print(f"two body propagation of {len(orbits)} orbits in {timing} s") - - -def test_merge_traj(): - # Initialize logging - FORMAT = "%(levelname)s %(name)s %(filename)s:%(lineno)d\t%(message)s" - logging.basicConfig(format=FORMAT) - logging.getLogger().setLevel(logging.INFO) - - # Base path - root = Path(__file__).joinpath("../../../").resolve() - - config_path = root.joinpath("./data/tests/config/") - - sc1 = Spacecraft.load(str(config_path.joinpath("spacecraft.yaml"))) - - dynamics = SpacecraftDynamics.load_named(str(config_path.joinpath("dynamics.yaml")))["lofi"] - - # Check loading from the YAML read from Python - with open(config_path.joinpath("dynamics.yaml")) as fh: - data = yaml.safe_load(fh) - - loaded = SpacecraftDynamics.loads(data["lofi"]) - assert loaded == dynamics - - sc2, traj1 = propagate(sc1, dynamics, Unit.Day * 5) - # And propagate again - sc3, traj2 = propagate(sc2, dynamics, Unit.Day * 5) - # Add the trajectories - traj = traj1 + traj2 - - assert traj.last().epoch == sc3.epoch, f"{traj.last()} != {sc3}" - - # Convert into another frame and try to add them too. - # We only check the epoch this time. - traj1_moon = traj1.to_frame("Moon J2000") - traj2_moon = traj2.to_frame("Moon J2000") - - traj_moon = traj1_moon + traj2_moon - - assert traj_moon.last().epoch == sc3.epoch - print(traj_moon) - - -if __name__ == "__main__": - test_propagate() - test_merge_traj() diff --git a/tests/python/test_orbit_determination.py b/tests/python/test_orbit_determination.py deleted file mode 100644 index 78159091..00000000 --- a/tests/python/test_orbit_determination.py +++ /dev/null @@ -1,379 +0,0 @@ -import logging -import pickle -import sys -from pathlib import Path - -import numpy as np -import pandas as pd -import yaml -from nyx_space.analysis import diff_traj_parquet -from nyx_space.cosmic import Spacecraft -from nyx_space.mission_design import SpacecraftDynamics, TrajectoryLoader, propagate -from nyx_space.orbit_determination import ( - DynamicTrackingArc, - ExportCfg, - GroundStation, - GroundTrackingArcSim, - OrbitEstimate, - TrkConfig, - predictor, - process_tracking_arc, -) -from nyx_space.plots.od import ( - plot_covar, - plot_estimates, - plot_residual_histogram, - plot_residuals, -) -from nyx_space.plots.traj import plot_orbit_elements -from nyx_space.time import TimeSeries, Unit - - -def test_filter_arc(): - # Initialize logging - FORMAT = "%(levelname)s %(name)s %(asctime)-15s %(filename)s:%(lineno)d %(message)s" - logging.basicConfig(format=FORMAT) - logging.getLogger().setLevel(logging.DEBUG) - - # Base path - root = Path(__file__).joinpath("../../../").resolve() - config_path = root.joinpath("./data/tests/config/") - outpath = root.joinpath("output_data/") - - # Load the dynamics and spacecraft - sc = Spacecraft.load(str(config_path.joinpath("spacecraft.yaml"))) - dynamics = SpacecraftDynamics.load_named(str(config_path.joinpath("dynamics.yaml"))) - - # An propagate for two periods (we only care about the trajectory) - _, traj = propagate(sc, dynamics["hifi"], sc.orbit.period().unwrap() * 2) - # Resample the trajectory at fixed step size - traj = traj.resample(Unit.Second * 10.0) - # And save the trajectory - traj_file = str(outpath.joinpath("./python_ref_traj.parquet")) - traj.to_parquet(traj_file) - - # Now starts the measurement generation - - # Load the devices - devices = GroundStation.load_many( - str(root.joinpath("./data/tests/config/many_ground_stations.yaml")) - ) - print(f"Loaded {devices}") - for device in devices: - # Check that we have loaded them correctly - if device.name == "Demo ground station": - assert device.latitude_deg == 2.3522 - assert device.longitude_deg == 48.8566 - else: - # This is Canberra, inherited from the build-in ground station - assert device.latitude_deg == -35.398333 - assert device.longitude_deg == 148.981944 - assert device.elevation_mask_deg == 5.0 # Both have the same mask - - # Load the tracking configuration as a dictionary - trk_cfg = TrkConfig.load_named(str(config_path.joinpath("tracking_cfg.yaml"))) - # Check that we can pickle and compare - trk_cfg_demo = trk_cfg["Demo ground station"] - print(trk_cfg_demo.dumps()) - unpkld = pickle.loads(pickle.dumps(trk_cfg_demo)) - assert unpkld == trk_cfg_demo - # Check that we can also call the `.loads` function directly - assert TrkConfig.loads(trk_cfg_demo.dumps()) == trk_cfg_demo - - # Load the trajectory - traj = TrajectoryLoader(traj_file) - print(f"Loaded {traj}") - - # Set up the export -- We'll use the same config set up for both measurements and output of OD process - cfg = ExportCfg(timestamp=True, metadata={"test key": "test value"}) - - # Build the simulated tracking arc, setting the seed to zero - arc_sim = GroundTrackingArcSim(devices, traj, trk_cfg, 0) - # Generate the measurements - print(arc_sim.generate_schedule()) - arc_sim.build_schedule() - msr_path = arc_sim.generate_measurements(str(outpath.joinpath("./msr.parquet")), cfg) - print(f"Saved {arc_sim} to {msr_path}") - - # Now let's filter this same data. - # Load the tracking arc - arc = DynamicTrackingArc(msr_path) - - # Create the orbit estimate with the covariance diagonal (100 km on position and 1 km/s on velocity) - orbit_est = OrbitEstimate(sc.orbit, covar=np.diag([100.0, 100.0, 100.0, 1.0, 1.0, 1.0])) - - # Check loading from the YAML read from Python - with open(config_path.joinpath("orbit_estimates.yaml")) as fh: - data = yaml.safe_load(fh) - - loaded = OrbitEstimate.loads(data["example 1"]) - print(loaded) - - msr_noise = [1e-3, 0, 0, 1e-6] - # Switch from sequential to EKF after 100 measurements - ekf_num_msr_trig = 100 - # Unless there is a 2 hour gap in the measurements, and then switch back to classical - ekf_disable_time = Unit.Hour * 2 - - rslt_path = process_tracking_arc( - dynamics["hifi"], - sc, - orbit_est, - msr_noise, - arc, - str(outpath.joinpath("./od_result.parquet")), - cfg, - ekf_num_msr_trig, - ekf_disable_time, - # predict_for=Unit.Hour * 12, # You can predict from the final estimate by uncommenting this line. - ) - - print(f"Stored to {rslt_path}") - - # Repeat with SNC to compare results - snc_rslt_path = process_tracking_arc( - dynamics["hifi"], - sc, - orbit_est, - msr_noise, - arc, - str(outpath.joinpath("./od_result_snc.parquet")), - cfg, - ekf_num_msr_trig, - ekf_disable_time, - snc_disable_time=Unit.Minute * 10.0, - snc_diagonals=[5e-12, 5e-12, 5e-12], - ) - - print(f"Stored to {rslt_path}") - - # Load the results - oddf = pd.read_parquet(rslt_path) - oddf_snc = pd.read_parquet(snc_rslt_path) - # Load the reference trajectory - ref_traj = pd.read_parquet(traj_file) - # Load the measurements - msr_df = pd.read_parquet(msr_path) - - # There seems to be a bug when exporting the HTML on Github action windows - # cf. https://github.com/nyx-space/nyx/actions/runs/5064848025/jobs/9092830624 - if sys.platform != "win32": - # We'll plot the difference between the reference trajectory and the OD results, with the measurement bands overlaid. - plot_estimates( - oddf, - "OD results from Python", - cov_frame="RIC", - ref_traj=ref_traj, - msr_df=msr_df, - html_out=str(outpath.joinpath("./od_estimate_plots.html")), - show=False, - ) - - plot_estimates( - oddf_snc, - "OD with SNC results from Python", - cov_frame="RIC", - ref_traj=ref_traj, - msr_df=msr_df, - html_out=str(outpath.joinpath("./od_estimate_snc_plots.html")), - show=False, - ) - - # Let's also plot the reference and the OD result's orbital elements - plot_orbit_elements( - [ref_traj, oddf], - "Python OD result", - ["Reference", "OD"], - html_out=str(outpath.joinpath("./od_vs_ref_elements.html")), - show=False, - ) - - # More often, the covariance is a better indicator - plot_covar( - oddf, - "OD 1-sigma covar from Python", - cov_sigma=1.0, - msr_df=msr_df, - html_out=str(outpath.joinpath("./od_covar_plots.html")), - show=False, - ) - - # Now, we'll plot the prefit residuals - plot_residuals( - oddf, - "OD residuals", - msr_df=msr_df, - html_out=str(outpath.joinpath("./od_residual_plots.html")), - show=False, - ) - plot_residuals( - oddf_snc, - "OD residuals with SNC enabled", - msr_df=msr_df, - html_out=str(outpath.joinpath("./od_residual_snc_plots.html")), - show=False, - ) - # And the postfit histograms - plot_residual_histogram( - oddf, - "OD residuals", - kind="Postfit", - html_out=str(outpath.joinpath("./od_residual_histograms.html")), - show=False, - ) - - # Plot the errors between the nominal and estimation - err_df = diff_traj_parquet(traj_file, rslt_path) - plot_orbit_elements( - err_df, - "Error in orbital elements", - html_out=str(outpath.joinpath("./od_vs_ref_error_elements.html")), - show=False, - ) - - err_snc_df = diff_traj_parquet(traj_file, snc_rslt_path) - plot_orbit_elements( - err_snc_df, - "Error in orbital elements with SNC", - html_out=str(outpath.joinpath("./od_snc_vs_ref_error_elements.html")), - show=False, - ) - - -def test_one_way_msr(): - """ - Test that we can manually perform a one-way measurement - """ - - # Base path - root = Path(__file__).joinpath("../../../").resolve() - config_path = root.joinpath("./data/tests/config/") - - # Load the dynamics and spacecraft - sc = Spacecraft.load(str(config_path.joinpath("spacecraft.yaml"))) - dynamics = SpacecraftDynamics.load_named(str(config_path.joinpath("dynamics.yaml"))) - - # Load the devices - devices = GroundStation.load_many(str(config_path.joinpath("./many_ground_stations.yaml"))) - print(f"Loaded {devices}") - - # One way measurement - - end_sc, traj = propagate(sc, dynamics["hifi"], sc.orbit.period().unwrap() * 1.1) - print(end_sc) - print(traj) - - # Let's build a dataframe of the range, doppler, azimuth, and elevation as seen from a ground station that sees the spacecraft a bunch - gs = devices[1] - print(f"Using {gs}") - cosm = Cosm.de438() - data = { - "epoch": [], - "range (km)": [], - "doppler (km/s)": [], - "azimuth (deg)": [], - "elevation (deg)": [], - } - # Start by building a time series - ts = TimeSeries(traj.first().epoch, traj.last().epoch, step=Unit.Minute * 30, inclusive=True) - # And iterate over it - for epoch in ts: - orbit = traj.at(epoch).orbit - try: - range_km, doppler_km_s = gs.measure(orbit) - except: - # Spacecraft is not visible then, nothing to store - pass - else: - # Also grab the azimuth and elevation angles - az_deg, el_deg = gs.compute_azimuth_elevation(orbit, cosm) - # And push to the data dictionary - data["epoch"] += [str(epoch)] - data["azimuth (deg)"] += [az_deg] - data["elevation (deg)"] += [el_deg] - data["range (km)"] += [range_km] - data["doppler (km/s)"] += [doppler_km_s] - # And convert to a data frame - df = pd.DataFrame(data, columns=data.keys()) - assert len(df) == 8 - print(df.describe()) - - # Test values - range_km, doppler_km_s = devices[0].measure(end_sc.orbit) - - print(range_km, doppler_km_s) - assert abs(range_km - 18097.562811514355) < 0.1 - assert abs(doppler_km_s - -0.2498238312640348) < 0.1 - - # Azimuth and elevation - - az_deg, el_deg = devices[0].compute_azimuth_elevation(end_sc.orbit, cosm) - - assert abs(az_deg - 128.66181520071825) < 1e-10 - assert abs(el_deg - 27.904687635388676) < 1e-10 - - -def test_pure_prediction(): - # Initialize logging - FORMAT = "%(levelname)s %(name)s %(asctime)-15s %(filename)s:%(lineno)d %(message)s" - logging.basicConfig(format=FORMAT) - logging.getLogger().setLevel(logging.INFO) - - # Base path - root = Path(__file__).joinpath("../../../").resolve() - config_path = root.joinpath("./data/tests/config/") - outpath = root.joinpath("output_data/") - - # Load the dynamics and spacecraft - sc = Spacecraft.load(str(config_path.joinpath("spacecraft.yaml"))) - dynamics = SpacecraftDynamics.load_named(str(config_path.joinpath("dynamics.yaml"))) - - # Set up the export -- We'll use the same config set up for both measurements and output of OD process - cfg = ExportCfg(timestamp=True, metadata={"test key": "test value"}) - - # We'll assume that we have a good estimate of the spacecraft's orbit before we predict it forward in time - # Hence, create the orbit estimate with the covariance diagonal (100 m on position and 50 m/s on velocity) - orbit_est = OrbitEstimate( - sc.orbit, - covar=np.diag([100.0e-3, 100.0e-3, 100.0e-3, 50.0e-3, 50.0e-3, 50.0e-3]), - ) - - rslt_path = predictor( - dynamics["hifi"], - sc, - orbit_est, - Unit.Second * 15.0, - str(outpath.joinpath("./od_pred_result.parquet")), - cfg, - predict_for=Unit.Hour * 12, - ) - - print(f"Stored to {rslt_path}") - - # Load the prediction results - oddf = pd.read_parquet(rslt_path) - - # There seems to be a bug when exporting the HTML on Github action windows - # cf. https://github.com/nyx-space/nyx/actions/runs/5064848025/jobs/9092830624 - if sys.platform != "win32": - # Let's plot the OD result's orbital elements - plot_orbit_elements( - oddf, - "OD prediction result", - html_out=str(outpath.joinpath("./od_pred_elements.html")), - show=False, - ) - - # More often, the covariance is a better indicator - plot_covar( - oddf, - "OD 1-sigma covar from Python", - cov_sigma=1.0, - cov_frame="Earth J2000", - html_out=str(outpath.joinpath("./od_pred_covar_plots.html")), - show=False, - ) - - -if __name__ == "__main__": - test_filter_arc() diff --git a/tests/python/test_yaml.py b/tests/python/test_yaml.py deleted file mode 100644 index 63678b16..00000000 --- a/tests/python/test_yaml.py +++ /dev/null @@ -1,150 +0,0 @@ -from nyx_space.orbit_determination import GroundStation, GaussMarkov, FltResid -from nyx_space.time import Unit - -import pickle - -gs_data = { - "one": { - "name": "Santiago, CL", - "frame": "IAU Earth", - "elevation_mask_deg": 5.0, - "range_noise_km": { - "tau": "24 h", - "bias_sigma": 0.005, - "steady_state_sigma": 0.0001, - }, - "doppler_noise_km_s": { - "tau": "24h", - "bias_sigma": 5e-05, - "steady_state_sigma": 1.5e-06, - }, - "light_time_correction": False, - "latitude_deg": -33.447487, - "longitude_deg": -70.673676, - "height_km": 0.5, - }, - "two": { - "name": "South Point, Hawaii, US", - "frame": "IAU Earth", - "latitude_deg": 18.911057, - "longitude_deg": -155.681022, - "height_km": 0.5, - "elevation_mask_deg": 5.0, - "range_noise_km": { - "tau": "24 h", - "bias_sigma": 0.005, - "steady_state_sigma": 0.0001, - }, - "doppler_noise_km_s": { - "tau": "24 h", - "bias_sigma": 5e-05, - "steady_state_sigma": 1.5e-06, - }, - "light_time_correction": False, - }, - "three": { - "name": "Dongara, AU", - "frame": "IAU Earth", - "latitude_deg": -29.251281, - "longitude_deg": 114.934621, - "height_km": 0.5, - "elevation_mask_deg": 5.0, - "range_noise_km": { - "tau": "24 h", - "bias_sigma": 0.005, - "steady_state_sigma": 0.0001, - }, - "doppler_noise_km_s": { - "tau": "24 h", - "bias_sigma": 5e-05, - "steady_state_sigma": 1.5e-06, - }, - "light_time_correction": False, - }, - "four": { - "name": "Maspalomas, ES", - "frame": "IAU Earth", - "latitude_deg": 27.760562, - "longitude_deg": -15.586017, - "height_km": 0.5, - "elevation_mask_deg": 5.0, - "range_noise_km": { - "tau": "24 h", - "bias_sigma": 0.005, - "steady_state_sigma": 0.0001, - }, - "doppler_noise_km_s": { - "tau": "24 h", - "bias_sigma": 5e-05, - "steady_state_sigma": 1.5e-06, - }, - "light_time_correction": False, - }, -} - -gm_data = { - "range_noise_km": { - "tau": "24 h", - "bias_sigma": 0.005, - "steady_state_sigma": 0.0001, - }, - "doppler_noise_km_s": { - "tau": "24h", - "bias_sigma": 5e-05, - "steady_state_sigma": 1.5e-06, - }, -} - - -def test_ground_station(): - # Test that we can load a ground station from a list - from_list = GroundStation.loads(list(gs_data.values())) - assert len(from_list) == 4 - - # Test that we can load a ground station from a dict - from_dict = GroundStation.loads(gs_data) - assert len(from_dict) == 4 - - # Test that we can load a ground station from a single dict definition - fourth = gs_data["four"] - fourth.pop("range_noise_km") - fourth.pop("doppler_noise_km_s") - # Using the constructor - unique = GroundStation(**fourth) - assert unique.name == "Maspalomas, ES" - - # Check pickle - pkld = pickle.dumps(unique) - unpkld = pickle.loads(pkld) - assert unpkld == unique - - -def test_gauss_markov(): - from_dict = GaussMarkov.loads(gm_data) - assert len(from_dict) == 2 - - from_list = GaussMarkov.loads(list(gm_data.values())) - assert len(from_list) == 2 - - unique_range = GaussMarkov.loads(gm_data["range_noise_km"])[0] - assert unique_range.tau == Unit.Day * 1.0 - - # NOTE: We can only pickle via the `dumps` and `loads` functions. - # This looses the stateful info. - - pkld = pickle.dumps(unique_range) - unpkl = pickle.loads(pkld) - assert unpkl.tau == Unit.Day * 1.0 - - -def test_flt_resid(): - # Test pickle - flt = FltResid(min_accepted=10, num_sigmas=3.0) - unplkd = pickle.loads(pickle.dumps(flt)) - assert unplkd == flt - - -if __name__ == "__main__": - test_ground_station() - test_gauss_markov() - test_flt_resid()