diff --git a/examples/spp/main.rs b/examples/spp/main.rs index b89e79b..7aef564 100644 --- a/examples/spp/main.rs +++ b/examples/spp/main.rs @@ -8,7 +8,7 @@ use gnss_rtk::prelude::{ // Orbit source example struct Orbits {} -impl OrbitSource for Orbits { +impl OrbitSource for &mut Orbits { // For each requested "t" and "sv", // if we can, we should resolve the SV [Orbit]. // If interpolation is to be used (depending on your apps), you can @@ -66,7 +66,7 @@ impl MyDataSource { pub fn main() { // Build the Orbit source - let orbits = Orbits {}; + let mut orbits = Orbits {}; // The preset API is useful to quickly deploy depending on your application. // Static presets target static positioning. @@ -79,7 +79,6 @@ pub fn main() { // We deploy without apriori knowledge. // The solver will initialize itself. None, // Tie the Orbit source - orbits, ); // The solver needs to be mutable, due to the iteration process. @@ -89,7 +88,7 @@ pub fn main() { // Browse your data source (This is an Example) while let Some((epoch, candidates)) = source.next() { - match solver.resolve(epoch, &candidates) { + match solver.resolve(epoch, &candidates, &mut orbits) { Ok((_epoch, solution)) => { // Receiver offset to preset timescale let (_clock_offset, _timescale) = (solution.dt, solution.timescale); diff --git a/src/cfg/method.rs b/src/cfg/method.rs index ad865ff..ccf7bd4 100644 --- a/src/cfg/method.rs +++ b/src/cfg/method.rs @@ -1,11 +1,11 @@ use crate::prelude::Error; #[cfg(feature = "serde")] -use serde::Deserialize; //, Serialize}; +use serde::{Deserialize, Serialize}; /// Solving method #[derive(Default, Debug, Clone, Copy, PartialEq)] -#[cfg_attr(feature = "serde", derive(Deserialize))] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub enum Method { /// Single Point Positioning (SPP). /// Code based navigation on a single carrier frequency. diff --git a/src/cfg/mod.rs b/src/cfg/mod.rs index 9170c68..e795744 100644 --- a/src/cfg/mod.rs +++ b/src/cfg/mod.rs @@ -22,7 +22,7 @@ pub enum Error { /// Geometry strategy #[derive(Default, Debug, Clone, Copy, PartialEq)] -#[cfg_attr(feature = "serde", derive(Deserialize))] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub enum GeometryStrategy { /// Algorithm selects best elevation angles #[default] @@ -35,7 +35,7 @@ pub enum GeometryStrategy { /// selects appropriate settings. Failing to select /// the apropriate [Profile] will degrade the solutions. #[derive(Default, Debug, Clone, Copy, PartialEq)] -#[cfg_attr(feature = "serde", derive(Deserialize))] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub enum Profile { /// Receiver held in static. /// Typically used in Geodetic surveys (GNSS stations Referencing) @@ -47,7 +47,7 @@ pub enum Profile { } #[derive(Default, Debug, Clone, PartialEq)] -#[cfg_attr(feature = "serde", derive(Deserialize))] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct ElevationMappingFunction { /// a + b * e-elev/c pub a: f64, @@ -64,7 +64,7 @@ impl ElevationMappingFunction { } #[derive(Debug, Clone, PartialEq)] -#[cfg_attr(feature = "serde", derive(Deserialize))] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub enum WeightMatrix { /// a + b e-elev/c MappingFunction(ElevationMappingFunction), @@ -162,7 +162,7 @@ fn default_tdop_threshold() -> Option { } #[derive(Default, Debug, Clone, PartialEq)] -#[cfg_attr(feature = "serde", derive(Deserialize))] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] /// System Internal Delay as defined by BIPM in /// "GPS Receivers Accurate Time Comparison" : the (frequency dependent) /// time delay introduced by the combination of: @@ -178,7 +178,7 @@ pub struct InternalDelay { } #[derive(Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde", derive(Deserialize))] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct SolverOpts { /// GDOP threshold to invalidate ongoing GDOP #[cfg_attr(feature = "serde", serde(default = "default_gdop_threshold"))] @@ -211,7 +211,7 @@ impl Default for SolverOpts { } #[derive(Default, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde", derive(Deserialize))] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct FilterOpts { /// Weight Matrix #[cfg_attr(feature = "serde", serde(default = "default_weight_matrix"))] @@ -302,8 +302,8 @@ impl Default for Modeling { } } -#[derive(Default, Debug, Clone, PartialEq)] -#[cfg_attr(feature = "serde", derive(Deserialize))] +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct Config { /// Type of solutions to form. #[cfg_attr(feature = "serde", serde(default))] @@ -440,3 +440,30 @@ impl Config { s } } + +impl Default for Config { + fn default() -> Self { + Self { + interp_order: 11, + max_tropo_bias: 30.0, + max_iono_bias: 30.0, + min_sv_elev: Some(10.0), + timescale: TimeScale::GPST, + sol_type: Default::default(), + method: Default::default(), + profile: Default::default(), + remote_site: Default::default(), + fixed_altitude: Default::default(), + code_smoothing: Default::default(), + int_delay: Default::default(), + arp_enu: Default::default(), + solver: Default::default(), + externalref_delay: Default::default(), + max_sv_occultation_percent: Default::default(), + min_sv_azim: Default::default(), + max_sv_azim: Default::default(), + min_snr: Default::default(), + modeling: Default::default(), + } + } +} diff --git a/src/navigation/filter.rs b/src/navigation/filter.rs index 4689f9a..bdcc9ae 100644 --- a/src/navigation/filter.rs +++ b/src/navigation/filter.rs @@ -1,14 +1,14 @@ use nalgebra::{base::dimension::U8, OMatrix, OVector, Vector3}; #[cfg(feature = "serde")] -use serde::Deserialize; +use serde::{Deserialize, Serialize}; use super::{Input, Output}; use crate::prelude::{Epoch, Error}; /// Navigation Filter. #[derive(Default, Debug, Clone, Copy, PartialEq)] -#[cfg_attr(feature = "serde", derive(Deserialize))] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub enum Filter { /// None: solver filter completely bypassed. Lighter calculations, no iterative behavior. None, diff --git a/src/navigation/solutions/mod.rs b/src/navigation/solutions/mod.rs index 804ca9a..90ccc57 100644 --- a/src/navigation/solutions/mod.rs +++ b/src/navigation/solutions/mod.rs @@ -14,10 +14,10 @@ pub use validator::InvalidationCause; pub type InstrumentBias = HashMap<(SV, Carrier), f64>; #[cfg(feature = "serde")] -use serde::Deserialize; +use serde::{Deserialize, Serialize}; #[derive(Debug, Copy, Clone, PartialEq, Default)] -#[cfg_attr(feature = "serde", derive(Deserialize))] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub enum PVTSolutionType { /// Default, complete solution with Position, /// Velocity and Time components. Requires either diff --git a/src/solver.rs b/src/solver.rs index fd64d4c..ee537c0 100644 --- a/src/solver.rs +++ b/src/solver.rs @@ -151,9 +151,7 @@ pub enum Error { } /// [Solver] to resolve [PVTSolution]s. -pub struct Solver { - /// [OrbitSource] - orbit: O, +pub struct Solver { /// Solver parametrization pub cfg: Config, /// Initial [Orbit] either forwarded by User @@ -215,7 +213,7 @@ fn signal_quality_filter(min_snr: f64, pool: &mut Vec) { }) } -impl Solver { +impl Solver { const ALMANAC_LOCAL_STORAGE: &str = ".cache"; fn nyx_anise_de440s_bsp() -> MetaFile { @@ -319,10 +317,11 @@ impl Solver { pub fn new_almanac_frame( cfg: &Config, initial: Option, - orbit: O, almanac: Almanac, frame: Frame, ) -> Self { + debug!("Deployed with {:#?}", cfg); + // Print more information if cfg.method == Method::SPP && cfg.max_sv_occultation_percent.is_some() { warn!("occultation filter is not meaningful in SPP mode"); @@ -339,7 +338,6 @@ impl Solver { // let almanac = Arc::new(almanac); Self { - orbit, almanac, earth_cef: frame, initial, @@ -362,31 +360,30 @@ impl Solver { /// You have to take that into account, especially when operating in Fixed Altitude /// or Time Only modes. /// - orbit: [OrbitSource] must be provided for Direct (1D) PPP - pub fn new(cfg: &Config, initial: Option, orbit: O) -> Result { + pub fn new(cfg: &Config, initial: Option) -> Result { let (almanac, earth_cef) = Self::build_almanac_frame_model()?; - Ok(Self::new_almanac_frame( - cfg, initial, orbit, almanac, earth_cef, - )) + Ok(Self::new_almanac_frame(cfg, initial, almanac, earth_cef)) } /// Create new Position [Solver] without knowledge of apriori position (full survey) - pub fn new_survey(cfg: &Config, orbit: O) -> Result { - Self::new(cfg, None, orbit) + pub fn new_survey(cfg: &Config) -> Result { + Self::new(cfg, None) } /// Create new Position [Solver] without knowledge of apriori position (full survey) /// and prefered [Almanac] and [Frame] to work with - pub fn new_survey_almanac_frame( - cfg: &Config, - orbit: O, - almanac: Almanac, - frame: Frame, - ) -> Self { - Self::new_almanac_frame(cfg, None, orbit, almanac, frame) + pub fn new_survey_almanac_frame(cfg: &Config, almanac: Almanac, frame: Frame) -> Self { + Self::new_almanac_frame(cfg, None, almanac, frame) } + /// [PVTSolution] resolution attempt. /// ## Inputs /// - t: desired [Epoch] /// - pool: list of [Candidate] - pub fn resolve(&mut self, t: Epoch, pool: &[Candidate]) -> Result<(Epoch, PVTSolution), Error> { + pub fn resolve( + &mut self, + t: Epoch, + pool: &[Candidate], + ref mut orbit: O, + ) -> Result<(Epoch, PVTSolution), Error> { let min_required = self.min_sv_required(); if pool.len() < min_required { // no need to proceed further @@ -421,10 +418,8 @@ impl Solver { .iter() .filter_map(|cd| match cd.transmission_time(&self.cfg) { Ok((t_tx, dt_tx)) => { - let orbits = &mut self.orbit; debug!("{} ({}) : signal propagation {}", cd.t, cd.sv, dt_tx); - if let Some(tx_orbit) = - orbits.next_at(t_tx, cd.sv, self.earth_cef, interp_order) + if let Some(tx_orbit) = orbit.next_at(t_tx, cd.sv, self.earth_cef, interp_order) { let orbit = Self::rotate_orbit_dcm3x3( cd.t, @@ -556,7 +551,10 @@ impl Solver { if retained { debug!("{}({}) - tropo delay {:.3E}[m]", cd.t, cd.sv, cd.tropo_bias); } else { - debug!("{}({}) - rejected (extreme tropo delay)", cd.t, cd.sv); + debug!( + "{}({}) - rejected (extreme tropo delay={:.3e})", + cd.t, cd.sv, cd.tropo_bias + ); } retained }); diff --git a/src/tests/mod.rs b/src/tests/mod.rs index 65c0e50..88c3c4f 100644 --- a/src/tests/mod.rs +++ b/src/tests/mod.rs @@ -3,16 +3,35 @@ use crate::prelude::{ Solver, TimeScale, SV, }; +use std::cell::RefCell; + mod bancroft; mod data; mod pseudo_range; mod pvt; +pub mod cfg; +use cfg::TestConfig; + +pub mod output; +use output::TestOutput; + +use std::collections::HashMap; + use data::{gps::test_data as gps_test_data, interp::interp_data}; -struct Orbits {} +#[derive(Debug, Clone)] +pub struct OrbitKey { + t: Epoch, + sv: SV, +} -impl OrbitSource for Orbits { +#[derive(Debug, Clone)] +struct OrbitDataBase { + inner: HashMap, +} + +impl OrbitSource for OrbitDataBase { fn next_at(&mut self, t: Epoch, sv: SV, fr: Frame, _: usize) -> Option { Some( interp_data() @@ -29,81 +48,46 @@ struct SolverInput { pool: Vec, } -#[derive(Debug, Default, Clone)] -struct Tester { - kinematic: bool, - timescale: TimeScale, - max_gdop: Option, - max_tdop: Option, - reference: Option, - max_xyz_err_m: (f64, f64, f64), - max_velocity_m_s: (f64, f64, f64), +struct Test { + pub cfg: TestConfig, + pub input: Vec, + pub orbit_db: OrbitDataBase, + pub solver: Solver, } -impl Tester { - /// Builds new Static Survey tester, for given ECEF [m] - pub fn static_survey( - timescale: TimeScale, - reference: Orbit, - max_xyz_err_m: (f64, f64, f64), - ) -> Self { - let mut s = Self::default(); - s.kinematic = false; - s.timescale = timescale; - s.max_xyz_err_m = max_xyz_err_m; - s.reference = Some(reference); - // on static applications, we tolerate this "erroneous" motion - s.max_velocity_m_s = (1.0E-5, 1.0E-5, 1.0E-5); - s - } - /// Set max tdop criteria - pub fn with_max_tdop(&self, tdop: f64) -> Self { - let mut s = self.clone(); - s.max_tdop = Some(tdop); - s - } - /// Set max gdop criteria - pub fn with_max_gdop(&self, gdop: f64) -> Self { - let mut s = self.clone(); - s.max_gdop = Some(gdop); - s - } - pub fn deploy(&self, cfg: &Config) { - self.deploy_without_apriori(cfg); - if self.reference.is_some() { - self.deploy_with_apriori(cfg); +impl Test { + pub fn run_test(&mut self) -> TestOutput { + let mut output = TestOutput::default(); + + while let Some(input) = self.input.iter_mut().next() { + let (t_rx, pool) = (input.t_rx, &mut input.pool); + + println!("running test: {} [{}]", input.t_rx, pool.len()); + + if let Some(solution) = + Self::test_iter(&mut self.solver, t_rx, pool, self.orbit_db.clone()) + { + output.nb_solutions += 1; + } + + output.nb_iter += 1; } + + output } - fn deploy_without_apriori(&self, cfg: &Config) { - let orbits = Orbits {}; - let mut solver = Solver::new_survey(&cfg, orbits) - .unwrap_or_else(|e| panic!("failed to deploy solver with {:#?}: error={}", cfg, e)); - println!("deployed with {:#?}", cfg); - self.run(&mut solver, cfg); - } - fn deploy_with_apriori(&self, cfg: &Config) { - let orbits = Orbits {}; - let mut solver = - Solver::new(&cfg, None, orbits) // TODO - .unwrap_or_else(|e| panic!("failed to deploy solver with {:#?}: error={}", cfg, e)); - println!("deployed with {:#?}", cfg); - self.run(&mut solver, cfg); - } - fn run(&self, solver: &mut Solver, cfg: &Config) { - for (data_index, data) in gps_test_data().iter_mut().enumerate() { - match solver.resolve(data.t_rx, &mut data.pool) { - Ok((_, solution)) => { - let state = solution.state; - let state = state.to_cartesian_pos_vel(); - let (x_km, y_km, z_km, vel_x, vel_y, vel_z) = - (state[0], state[1], state[2], state[3], state[4], state[5]); - println!( - "iter={}, 3d=(x={}km, y={}km, z={}km) vel=(x={}km/s, y={}km/s, z={}km/s)", - data_index, x_km, y_km, z_km, vel_x, vel_y, vel_z, - ); - self.static_run(&cfg, solution); - }, - Err(e) => match e { + + fn test_iter( + solver: &mut Solver, + t_rx: Epoch, + pool: &mut Vec, + orbit: OrbitDataBase, + ) -> Option { + match solver.resolve(t_rx, pool, orbit) { + Ok((_, solution)) => { + return Some(solution); + }, + Err(e) => { + match e { Error::NotEnoughCandidates => {}, Error::NotEnoughCandidatesBancroft => {}, Error::NotEnoughPreFitCandidates => {}, @@ -158,46 +142,9 @@ impl Tester { Error::EarthFrame(e) => { panic!("earth frame error: {}", e); }, - }, - } + } + }, } - } - fn static_run(&self, _cfg: &Config, sol: PVTSolution) { - //let reference = self.reference.as_ref().unwrap(); - //// let (x0, y0, z0) = (xyz_ecef_m[0], xyz_ecef_m[1], xyz_ecef_m[2]); - //let orbit = sol.state; - //let state = orbit.to_cartesian_pos_vel(); - //let (x_km, y_km, z_km, vel_x_km, vel_y_km, vel_z_km) = ( - // state[0], - // state[1], - // state[2], - // state[3], - // state[4], - // state[5], - //); - //let (x_err, y_err, z_err) = ((x_km * 1.0E3 - x0).abs(), (y_km * 1.0E3 - y0).abs(), (z_km * 1.0E3 - z0).abs()); - assert_eq!( - sol.timescale, self.timescale, - "solution expressed in wrong timescale" - ); - //if let Some(max_gdop) = self.max_gdop { - // assert!( - // sol.gdop.abs() < max_gdop, - // "{} gdop limit exceeded", - // max_gdop - // ); - //} - //if let Some(max_tdop) = self.max_tdop { - // assert!( - // sol.tdop.abs() < max_tdop, - // "{} tdop limit exceeded", - // max_tdop - // ); - //} - //assert!( - // vel_x_km.abs() <= self.max_velocity_m_s.0, - // "{} vel_x component above tolerance", - // vel_x_km.abs() - //); + None } } diff --git a/src/tests/pvt/spp.rs b/src/tests/pvt/spp.rs index 314b28a..71ec847 100644 --- a/src/tests/pvt/spp.rs +++ b/src/tests/pvt/spp.rs @@ -1,6 +1,6 @@ use crate::{ prelude::{Config, Epoch, Filter, Method, Orbit, PVTSolutionType, TimeScale, EARTH_J2000}, - tests::Tester, + tests::Test, }; use std::str::FromStr; @@ -8,18 +8,18 @@ use std::str::FromStr; #[test] #[ignore] fn spp_lsq_static_survey() { - let orbit = Orbit::from_position( - 0.0, - 0.0, - 0.0, - Epoch::from_str("2020-06-25T00:00:00 GPST").unwrap(), - EARTH_J2000, - ); - let tester = Tester::static_survey(TimeScale::GPST, orbit, (1.0, 1.0, 1.0)); - let mut cfg = Config::static_ppp_preset(Method::SPP); - cfg.min_snr = None; - cfg.min_sv_elev = None; - cfg.solver.filter = Filter::LSQ; - cfg.sol_type = PVTSolutionType::PositionVelocityTime; - tester.deploy(&cfg); + // let orbit = Orbit::from_position( + // 0.0, + // 0.0, + // 0.0, + // Epoch::from_str("2020-06-25T00:00:00 GPST").unwrap(), + // EARTH_J2000, + // ); + // let tester = Tester::static_survey(TimeScale::GPST, orbit, (1.0, 1.0, 1.0)); + // let mut cfg = Config::static_ppp_preset(Method::SPP); + // cfg.min_snr = None; + // cfg.min_sv_elev = None; + // cfg.solver.filter = Filter::LSQ; + // cfg.sol_type = PVTSolutionType::PositionVelocityTime; + // tester.deploy(&cfg); }