diff --git a/README.md b/README.md index b42a07e3b..be207d626 100644 --- a/README.md +++ b/README.md @@ -20,19 +20,19 @@ You can also open a [Discussion](https://github.com/georust/rinex/discussions) o - Fast - Parse and render reports in a few seconds - - Resolve PVT solutions as well + - Perform precise Geodetic surveys in a few seconds - Open sources - Read and access all the code - All examples based on Open data -- Efficient seamless compression / decompression - - Hatanaka (OBS_RINEX) and Gzip are built in +- All modern GNSS constellations, codes and signals + - Surveying with GPS, Galileo, BeiDou and QZSS +- Time scales: GPST, QZSST, BDT, GST, UTC, TAI +- Efficient seamless compression and decompression - RINEX V4 full support - All RINEX formats supported (see following table) -- High Precision Clock RINEX products -- High Precision Orbital (SP3) products [SP3](https://docs.rs/sp3/1.0.7/sp3/) -- DORIS (special RINEX) by ESA -- All modern GNSS constellations, codes and signals -- Time scales: GPST, BDT, GST, UTC +- High Precision Clock RINEX products (for PPP) +- High Precision Orbital [SP3 for PPP](https://docs.rs/sp3/1.0.7/sp3/) +- DORIS (special RINEX) - Several pre-processing algorithms: - [File merging](https://github.com/georust/rinex/wiki/file-merging) - [Time binning](https://github.com/georust/rinex/wiki/time-binning) @@ -45,11 +45,8 @@ You can also open a [Discussion](https://github.com/georust/rinex/discussions) o ## Disadvantages :warning: -- QZSST is not supported until next release -- PPP is work in progress -- Navigation is only available in Galileo and GPS mode. -QZSST comes next. -BDS and IRNSS will either come at the same time or shortly after. +- Navigation is currently not feasible with Glonass and IRNSS +- Differential navigation (SBAS, DGNSS or RTK) is not support yet - Our applications do not accept BINEX or other proprietary formats - File production might lack some features, mostly because we're currently focused on data processing diff --git a/rinex-cli/Cargo.toml b/rinex-cli/Cargo.toml index 1cd4ad512..70d5e265d 100644 --- a/rinex-cli/Cargo.toml +++ b/rinex-cli/Cargo.toml @@ -30,20 +30,20 @@ map_3d = "0.1.5" colorous = "1.0" horrorshow = "0.8" clap = { version = "4.4.13", features = ["derive", "color"] } -hifitime = { version = "3.9.0", features = ["serde", "std"] } -gnss-rs = { version = "2.1.3" , features = ["serde"] } +hifitime = { git = "https://github.com/nyx-space/hifitime.git", branch = "master", features = ["serde", "std"] } +gnss-rs = { git = "https://github.com/rtk-rs/gnss", branch = "main", features = ["serde"] } rinex = { path = "../rinex", version = "=0.16.1", features = ["full"] } plotly = { git = "https://github.com/plotly/plotly.rs", branch = "main" } rinex-qc = { path = "../rinex-qc", version = "=0.1.14", features = ["serde"] } sp3 = { path = "../sp3", version = "=1.0.8", features = ["serde", "flate2"] } serde = { version = "1.0", default-features = false, features = ["derive"] } - # solver -gnss-rtk = { version = "0.4.5", features = ["serde"] } +# gnss-rtk = { version = "0.4.5", features = ["serde"] } # gnss-rtk = { path = "../../rtk-rs/gnss-rtk", features = ["serde"] } -# gnss-rtk = { git = "https://github.com/rtk-rs/gnss-rtk", branch = "main", features = ["serde"] } +gnss-rtk = { git = "https://github.com/rtk-rs/gnss-rtk", branch = "main", features = ["serde"] } # cggtts -cggtts = { version = "4.1.4", features = ["serde", "scheduler"] } +# cggtts = { version = "4.1.4", features = ["serde", "scheduler"] } # cggtts = { path = "../../cggtts/cggtts", features = ["serde", "scheduler"] } +cggtts = { git = "https://github.com/gwbres/cggtts", branch = "dev", features = ["serde", "scheduler"] } diff --git a/rinex-cli/config/rtk/gpst_cpp_basic.json b/rinex-cli/config/rtk/gpst_cpp_basic.json index 5f2050d8c..0b2e6d6fe 100644 --- a/rinex-cli/config/rtk/gpst_cpp_basic.json +++ b/rinex-cli/config/rtk/gpst_cpp_basic.json @@ -1,5 +1,5 @@ { - "method": "CodePPP", + "method": "CPP", "timescale": "GPST", "interp_order": 17, "min_sv_elevation": 5.0, diff --git a/rinex-cli/config/rtk/gpst_cpp_kf.json b/rinex-cli/config/rtk/gpst_cpp_kf.json index b34e87692..fd915184e 100644 --- a/rinex-cli/config/rtk/gpst_cpp_kf.json +++ b/rinex-cli/config/rtk/gpst_cpp_kf.json @@ -1,5 +1,5 @@ { - "method": "CodePPP", + "method": "CPP", "timescale": "GPST", "interp_order": 17, "min_sv_elev": 5.0, diff --git a/rinex-cli/src/graph/mod.rs b/rinex-cli/src/graph/mod.rs index 00c9dcaa8..064c071ed 100644 --- a/rinex-cli/src/graph/mod.rs +++ b/rinex-cli/src/graph/mod.rs @@ -515,6 +515,7 @@ fn gnss_combination_plot(matches: &ArgMatches) -> bool { /* Returns True if Navigation plot is to be generated */ fn navigation_plot(matches: &ArgMatches) -> bool { matches.get_flag("skyplot") + || matches.get_flag("orbit") || matches.get_flag("orbit-residual") || matches.get_flag("sv-clock") } diff --git a/rinex-cli/src/positioning/cggtts/mod.rs b/rinex-cli/src/positioning/cggtts/mod.rs index fb95bcc37..23eac3a74 100644 --- a/rinex-cli/src/positioning/cggtts/mod.rs +++ b/rinex-cli/src/positioning/cggtts/mod.rs @@ -10,9 +10,6 @@ use gnss::prelude::{Constellation, SV}; use rinex::{carrier::Carrier, prelude::Observable}; -use super::cast_rtk_carrier; -use super::interp::TimeInterpolator; - use rtk::prelude::{ Candidate, Duration, @@ -30,9 +27,16 @@ use cggtts::{ track::{FitData, GlonassChannel, SVTracker, Scheduler}, }; -use crate::cli::Context; -use crate::positioning::{ - bd_model, kb_model, ng_model, tropo_components, Error as PositioningError, +use crate::{ + cli::Context, + positioning::{ + bd_model, + cast_rtk_carrier, + kb_model, + ng_model, //tropo_components, + Error as PositioningError, + Time, + }, }; // fn reset_sv_tracker(sv: SV, trackers: &mut HashMap<(SV, Observable), SVTracker>) { @@ -61,7 +65,7 @@ use crate::positioning::{ pub fn resolve( ctx: &Context, mut solver: Solver, - rx_lat_ddeg: f64, + // rx_lat_ddeg: f64, matches: &ArgMatches, ) -> Result, PositioningError> where @@ -83,26 +87,13 @@ where // infaillible, at this point let obs_data = ctx.data.observation().unwrap(); let nav_data = ctx.data.brdc_navigation().unwrap(); - - let clk_data = ctx.data.clock(); - let meteo_data = ctx.data.meteo(); - - let sp3_has_clock = ctx.data.sp3_has_clock(); - if clk_data.is_none() && sp3_has_clock { - if let Some(sp3) = ctx.data.sp3() { - warn!("Using clock states defined in SP3 file - CLK product should be prefered"); - if sp3.epoch_interval >= Duration::from_seconds(300.0) { - warn!("Interpolating clock states from low sample rate SP3 will most likely introduce errors"); - } - } - } + // let meteo_data = ctx.data.meteo(); //TODO let dominant_sampling_period = obs_data .dominant_sample_rate() .expect("RNX2CGGTTS requires steady GNSS observations"); - let mut interp = TimeInterpolator::from_ctx(ctx); - debug!("Clock interpolator created"); + let mut time = Time::from_ctx(ctx); // CGGTTS specifics let mut tracks = Vec::::new(); @@ -120,8 +111,8 @@ where continue; } - // Nearest TROPO - let zwd_zdd = tropo_components(meteo_data, *t, rx_lat_ddeg); + // Nearest TROPO: TODO + // let zwd_zdd = tropo_components(meteo_data, *t, rx_lat_ddeg); for (sv, observations) in vehicles { let sv_eph = nav_data.sv_ephemeris(*sv, *t); @@ -134,7 +125,7 @@ where // determine TOE let (_toe, sv_eph) = sv_eph.unwrap(); - let clock_corr = match interp.next_at(*t, *sv) { + let clock_corr = match time.next_at(*t, *sv) { Some(dt) => dt, None => { error!("{:?} ({}) - failed to determine clock correction", *t, *sv); @@ -150,8 +141,8 @@ where }; let tropo_bias = TroposphereBias { - total: None, //TODO - zwd_zdd, + total: None, //TODO + zwd_zdd: None, // TODO }; // tries to form a candidate for each signal @@ -177,8 +168,8 @@ where // attach one phase, if need be match solver.cfg.method { - Method::SPP => {}, // nothing to do - Method::CodePPP => {}, // nothing to do + Method::SPP => {}, // nothing to do + Method::CPP => {}, // nothing to do Method::PPP => { // try to attach phase data // let to_match = @@ -220,7 +211,7 @@ where // complete if need be match solver.cfg.method { Method::SPP => {}, // nothing to do - Method::CodePPP => { + Method::CPP => { // Attach secondary PR for (second_obs, second_data) in observations { let rhs_carrier = diff --git a/rinex-cli/src/positioning/interp/mod.rs b/rinex-cli/src/positioning/interp.rs similarity index 70% rename from rinex-cli/src/positioning/interp/mod.rs rename to rinex-cli/src/positioning/interp.rs index 7e7d4c512..4d9f2a248 100644 --- a/rinex-cli/src/positioning/interp/mod.rs +++ b/rinex-cli/src/positioning/interp.rs @@ -1,38 +1,31 @@ -mod orbit; -pub use orbit::Interpolator as OrbitInterpolator; - -mod time; -pub use time::Interpolator as TimeInterpolator; - use rinex::prelude::{Duration, Epoch}; -/// Interpolators internal buffer. -/// Both interpolators, whether it be Temporal or Position, both work on 3D values represented as f64 double precision. -pub trait Buffer { +/// Interpolator buffer Trait. +pub trait Buffer { /// Memory allocation fn malloc(size: usize) -> Self; /// Return current number of symbols fn len(&self) -> usize; /// Return symbol by index - fn get(&self, index: usize) -> Option<&(Epoch, (f64, f64, f64))>; + fn get(&self, index: usize) -> Option<&(Epoch, T)>; /// Clear all symbols fn clear(&mut self); /// New new symbol - fn push(&mut self, x_j: (Epoch, (f64, f64, f64))); + fn push(&mut self, x_j: (Epoch, T)); /// Returns internal symbols - fn snapshot(&self) -> &[(Epoch, (f64, f64, f64))]; + fn snapshot(&self) -> &[(Epoch, T)]; /// Returns true if an interpolation of this order is feasible @ t fn feasible(&self, order: usize, t: Epoch) -> bool; /// Returns direct output in rare cases where Interpolation is not needed. /// This avoids introduction extra bias in the measurement, due to the interpolation process. - fn direct_output(&self, t: Epoch) -> Option<&(f64, f64, f64)> { + fn direct_output(&self, t: Epoch) -> Option<&T> { self.snapshot() .iter() .filter_map(|(k, v)| if *k == t { Some(v) } else { None }) .reduce(|k, _| k) } /// Returns mutable internal symbols - fn snapshot_mut(&mut self) -> &mut [(Epoch, (f64, f64, f64))]; + fn snapshot_mut(&mut self) -> &mut [(Epoch, T)]; /// Returns latest interval fn last_dt(&self) -> Option<(Epoch, Duration)> { if self.len() > 1 { @@ -44,10 +37,9 @@ pub trait Buffer { } } /// Streams data in, in chronological order with gap intolerance. - fn fill(&mut self, x_j: (Epoch, (f64, f64, f64))) { + fn fill(&mut self, x_j: (Epoch, T)) { if let Some((last, dt)) = self.last_dt() { if (x_j.0 - last).to_seconds().is_sign_positive() { - // NB Should we make gap tolerance more flexible ? if (x_j.0 - last) > dt { warn!("{} - {} gap detected - buffer reset", x_j.0, x_j.0 - last); self.clear(); diff --git a/rinex-cli/src/positioning/mod.rs b/rinex-cli/src/positioning/mod.rs index 7f6c0bc25..01f56c4d8 100644 --- a/rinex-cli/src/positioning/mod.rs +++ b/rinex-cli/src/positioning/mod.rs @@ -13,25 +13,28 @@ use cggtts::PostProcessingError as CGGTTSPostProcessingError; use clap::ArgMatches; use gnss::prelude::Constellation; // SV}; use rinex::carrier::Carrier; -use rinex::prelude::{Observable, Rinex}; +use rinex::prelude::Rinex; use rtk::prelude::{ - AprioriPosition, BdModel, Carrier as RTKCarrier, Config, Duration, Epoch, Error as RTKError, - KbModel, Method, NgModel, PVTSolutionType, Solver, Vector3, + BdModel, Carrier as RTKCarrier, Config, Duration, Epoch, Error as RTKError, KbModel, Method, + NgModel, PVTSolutionType, Solver, }; -use map_3d::{ecef2geodetic, rad2deg, Ellipsoid}; use thiserror::Error; +mod orbit; +pub use orbit::Orbit; + +mod time; +pub use time::Time; + mod interp; -use interp::OrbitInterpolator; +pub use interp::Buffer as BufferTrait; #[derive(Debug, Error)] pub enum Error { #[error("solver error")] SolverError(#[from] RTKError), - #[error("undefined apriori position")] - UndefinedAprioriPosition, #[error("ppp post processing error")] PPPPostProcessingError(#[from] PPPPostProcessingError), #[error("cggtts post processing error")] @@ -47,88 +50,98 @@ pub fn cast_rtk_carrier(carrier: Carrier) -> RTKCarrier { Carrier::L5 => RTKCarrier::L5, Carrier::L6 => RTKCarrier::L6, Carrier::E1 => RTKCarrier::E1, - Carrier::E5 | Carrier::E5a | Carrier::E5b => RTKCarrier::E5, + Carrier::E5 => RTKCarrier::E5, Carrier::E6 => RTKCarrier::E6, + Carrier::E5a => RTKCarrier::E5A, + Carrier::E5b => RTKCarrier::E5B, + Carrier::B1I => RTKCarrier::B1I, + Carrier::B2 => RTKCarrier::B2, + Carrier::B3 | Carrier::B3A => RTKCarrier::B3, + Carrier::B2A => RTKCarrier::B2A, + Carrier::B2I | Carrier::B2B => RTKCarrier::B2iB2b, + Carrier::B1A | Carrier::B1C => RTKCarrier::B1aB1c, Carrier::L1 | _ => RTKCarrier::L1, } } -pub fn tropo_components(meteo: Option<&Rinex>, t: Epoch, lat_ddeg: f64) -> Option<(f64, f64)> { - const MAX_LATDDEG_DELTA: f64 = 15.0; - let max_dt = Duration::from_hours(24.0); - let rnx = meteo?; - let meteo = rnx.header.meteo.as_ref().unwrap(); +//use map_3d::{ecef2geodetic, rad2deg, Ellipsoid}; - let delays: Vec<(Observable, f64)> = meteo - .sensors - .iter() - .filter_map(|s| match s.observable { - Observable::ZenithDryDelay => { - let (x, y, z, _) = s.position?; - let (lat, _, _) = ecef2geodetic(x, y, z, Ellipsoid::WGS84); - let lat = rad2deg(lat); - if (lat - lat_ddeg).abs() < MAX_LATDDEG_DELTA { - let value = rnx - .zenith_dry_delay() - .filter(|(t_sens, _)| (*t_sens - t).abs() < max_dt) - .min_by_key(|(t_sens, _)| (*t_sens - t).abs()); - let (_, value) = value?; - debug!("{:?} lat={} zdd {}", t, lat_ddeg, value); - Some((s.observable.clone(), value)) - } else { - None - } - }, - Observable::ZenithWetDelay => { - let (x, y, z, _) = s.position?; - let (mut lat, _, _) = ecef2geodetic(x, y, z, Ellipsoid::WGS84); - lat = rad2deg(lat); - if (lat - lat_ddeg).abs() < MAX_LATDDEG_DELTA { - let value = rnx - .zenith_wet_delay() - .filter(|(t_sens, _)| (*t_sens - t).abs() < max_dt) - .min_by_key(|(t_sens, _)| (*t_sens - t).abs()); - let (_, value) = value?; - debug!("{:?} lat={} zdd {}", t, lat_ddeg, value); - Some((s.observable.clone(), value)) - } else { - None - } - }, - _ => None, - }) - .collect(); - - if delays.len() < 2 { - None - } else { - let zdd = delays - .iter() - .filter_map(|(obs, value)| { - if obs == &Observable::ZenithDryDelay { - Some(*value) - } else { - None - } - }) - .reduce(|k, _| k) - .unwrap(); - - let zwd = delays - .iter() - .filter_map(|(obs, value)| { - if obs == &Observable::ZenithWetDelay { - Some(*value) - } else { - None - } - }) - .reduce(|k, _| k) - .unwrap(); - - Some((zwd, zdd)) - } -} +//pub fn tropo_components(meteo: Option<&Rinex>, t: Epoch, lat_ddeg: f64) -> Option<(f64, f64)> { +// const MAX_LATDDEG_DELTA: f64 = 15.0; +// let max_dt = Duration::from_hours(24.0); +// let rnx = meteo?; +// let meteo = rnx.header.meteo.as_ref().unwrap(); +// +// let delays: Vec<(Observable, f64)> = meteo +// .sensors +// .iter() +// .filter_map(|s| match s.observable { +// Observable::ZenithDryDelay => { +// let (x, y, z, _) = s.position?; +// let (lat, _, _) = ecef2geodetic(x, y, z, Ellipsoid::WGS84); +// let lat = rad2deg(lat); +// if (lat - lat_ddeg).abs() < MAX_LATDDEG_DELTA { +// let value = rnx +// .zenith_dry_delay() +// .filter(|(t_sens, _)| (*t_sens - t).abs() < max_dt) +// .min_by_key(|(t_sens, _)| (*t_sens - t).abs()); +// let (_, value) = value?; +// debug!("{:?} lat={} zdd {}", t, lat_ddeg, value); +// Some((s.observable.clone(), value)) +// } else { +// None +// } +// }, +// Observable::ZenithWetDelay => { +// let (x, y, z, _) = s.position?; +// let (mut lat, _, _) = ecef2geodetic(x, y, z, Ellipsoid::WGS84); +// lat = rad2deg(lat); +// if (lat - lat_ddeg).abs() < MAX_LATDDEG_DELTA { +// let value = rnx +// .zenith_wet_delay() +// .filter(|(t_sens, _)| (*t_sens - t).abs() < max_dt) +// .min_by_key(|(t_sens, _)| (*t_sens - t).abs()); +// let (_, value) = value?; +// debug!("{:?} lat={} zdd {}", t, lat_ddeg, value); +// Some((s.observable.clone(), value)) +// } else { +// None +// } +// }, +// _ => None, +// }) +// .collect(); +// +// if delays.len() < 2 { +// None +// } else { +// let zdd = delays +// .iter() +// .filter_map(|(obs, value)| { +// if obs == &Observable::ZenithDryDelay { +// Some(*value) +// } else { +// None +// } +// }) +// .reduce(|k, _| k) +// .unwrap(); +// +// let zwd = delays +// .iter() +// .filter_map(|(obs, value)| { +// if obs == &Observable::ZenithWetDelay { +// Some(*value) +// } else { +// None +// } +// }) +// .reduce(|k, _| k) +// .unwrap(); +// +// Some((zwd, zdd)) +// } +//} /* * Grabs nearest KB model (in time) @@ -203,14 +216,7 @@ pub fn precise_positioning(ctx: &Context, matches: &ArgMatches) -> Result<(), Er cfg }, }; - /* Verify requirements and print helpful comments */ - let apriori_ecef = ctx.rx_ecef.ok_or(Error::UndefinedAprioriPosition)?; - - let apriori = Vector3::::new(apriori_ecef.0, apriori_ecef.1, apriori_ecef.2); - let apriori = AprioriPosition::from_ecef(apriori); - let rx_lat_ddeg = apriori.geodetic()[0]; - assert!( ctx.data.observation().is_some(), "Positioning requires Observation RINEX" @@ -220,12 +226,6 @@ pub fn precise_positioning(ctx: &Context, matches: &ArgMatches) -> Result<(), Er "Positioning requires Navigation RINEX" ); - if cfg.interp_order > 5 && ctx.data.sp3().is_none() { - error!("High interpolation orders are likely incompatible with navigation based on broadcast radio."); - warn!("It is possible that this configuration does not generate any solutions."); - info!("Consider loading high precision SP3 data to use high interpolation orders."); - } - if let Some(obs_rinex) = ctx.data.observation() { if let Some(obs_header) = &obs_rinex.header.obs { if let Some(time_of_first_obs) = obs_header.time_of_first_obs { @@ -240,16 +240,23 @@ pub fn precise_positioning(ctx: &Context, matches: &ArgMatches) -> Result<(), Er } } } + } else if let Some(sp3) = ctx.data.sp3() { + if ctx.data.sp3_has_clock() { + if sp3.time_scale == time_of_first_obs.time_scale { + info!("Temporal PPP compliancy"); + } else { + error!("Working with different timescales in OBS/SP3 is not PPP compatible and will generate tiny errors"); + if sp3.epoch_interval >= Duration::from_seconds(300.0) { + warn!("Interpolating clock states from low sample rate SP3 will most likely introduce errors"); + } + } + } } } } } - let orbit = RefCell::new(OrbitInterpolator::from_ctx( - ctx, - cfg.interp_order, - apriori.clone(), - )); + let orbit = RefCell::new(Orbit::from_ctx(ctx, cfg.interp_order)); debug!("Orbit interpolator created"); // print config to be used @@ -257,20 +264,25 @@ pub fn precise_positioning(ctx: &Context, matches: &ArgMatches) -> Result<(), Er let solver = Solver::new( &cfg, - apriori, + None, /* state vector interpolator */ |t, sv, _order| orbit.borrow_mut().next_at(t, sv), )?; if matches.get_flag("cggtts") { /* CGGTTS special opmode */ - let tracks = cggtts::resolve(ctx, solver, rx_lat_ddeg, matches)?; + let tracks = cggtts::resolve(ctx, solver, matches)?; cggtts_post_process(ctx, tracks, matches)?; } else { /* PPP */ - let pvt_solutions = ppp::resolve(ctx, solver, rx_lat_ddeg); - /* save solutions (graphs, reports..) */ - ppp_post_process(ctx, pvt_solutions, matches)?; + let solutions = ppp::resolve(ctx, solver); + if solutions.len() > 0 { + /* save solutions (graphs, reports..) */ + ppp_post_process(ctx, solutions, matches)?; + } else { + error!("solver did not generate a single solution"); + error!("verify your input data and configuration setup"); + } } Ok(()) } diff --git a/rinex-cli/src/positioning/orbit/mod.rs b/rinex-cli/src/positioning/orbit/mod.rs new file mode 100644 index 000000000..a2c5bfc5b --- /dev/null +++ b/rinex-cli/src/positioning/orbit/mod.rs @@ -0,0 +1,29 @@ +use crate::cli::Context; +use gnss_rtk::prelude::{Epoch, InterpolationResult, SV}; + +mod sp3; +use sp3::Orbit as SP3Orbit; + +mod nav; +use nav::Orbit as NAVOrbit; + +pub enum Orbit<'a> { + SP3(SP3Orbit<'a>), + NAV(NAVOrbit<'a>), +} + +impl<'a> Orbit<'a> { + pub fn from_ctx(ctx: &'a Context, order: usize) -> Self { + if ctx.data.has_sp3() { + Self::SP3(SP3Orbit::from_ctx(ctx, order)) + } else { + Self::NAV(NAVOrbit::from_ctx(ctx)) + } + } + pub fn next_at(&mut self, t: Epoch, sv: SV) -> Option { + match self { + Self::SP3(orbit) => orbit.next_at(t, sv), + Self::NAV(orbit) => orbit.next_at(t, sv), + } + } +} diff --git a/rinex-cli/src/positioning/orbit/nav.rs b/rinex-cli/src/positioning/orbit/nav.rs new file mode 100644 index 000000000..cc0d70282 --- /dev/null +++ b/rinex-cli/src/positioning/orbit/nav.rs @@ -0,0 +1,92 @@ +use crate::cli::Context; +use std::collections::HashMap; + +use gnss_rtk::prelude::{Epoch, InterpolationResult as RTKInterpolationResult, TimeScale, SV}; + +use rinex::navigation::Ephemeris; + +pub struct Orbit<'a> { + buffer: HashMap>, + iter: Box + 'a>, +} + +impl<'a> Orbit<'a> { + pub fn from_ctx(ctx: &'a Context) -> Self { + let brdc = ctx + .data + .brdc_navigation() + .expect("BRDC navigation required"); + Self { + buffer: HashMap::with_capacity(64), + iter: Box::new(brdc.ephemeris().map(|(_toc, (_, sv, eph))| (sv, eph))), + } + } + fn feasible(&self, t: Epoch, sv: SV, sv_ts: TimeScale) -> bool { + let max_dtoe = Ephemeris::max_dtoe(sv.constellation).unwrap(); + if let Some(dataset) = self.buffer.get(&sv) { + let mut index = dataset.len(); + while index > 1 { + index -= 1; + let eph_i = &dataset[index]; + if let Some(toe) = eph_i.toe_gpst(sv_ts) { + if toe < t && (t - toe) < max_dtoe { + return true; + } + } + } + false + } else { + false + } + } + pub fn next_at(&mut self, t: Epoch, sv: SV) -> Option { + let sv_ts = sv.timescale()?; + + while !self.feasible(t, sv, sv_ts) { + if let Some((sv_i, eph_i)) = self.iter.next() { + if let Some(dataset) = self.buffer.get_mut(&sv_i) { + dataset.push(eph_i.clone()); + } else { + self.buffer.insert(sv_i, vec![eph_i.clone()]); + } + } else { + // EOF + return None; + } + } + + let output = match self.buffer.get(&sv) { + Some(eph) => { + let eph_i = eph.iter().min_by_key(|eph_i| { + let toe_i = eph_i.toe_gpst(sv_ts).unwrap(); + t - toe_i + })?; + let (x_km, y_km, z_km) = eph_i.kepler2ecef(sv, t)?; + let (x, y, z) = (x_km * 1.0E3, y_km * 1.0E3, z_km * 1.0E3); + Some(RTKInterpolationResult::from_position((x, y, z))) + }, + None => None, + }; + // TODO improve memory footprint: avoid memory growth + //self.buffer.retain(|sv_i, ephemeris| { + // if *sv_i == sv { + // let max_dtoe = Ephemeris::max_dtoe(sv.constellation) + // .unwrap() + // .to_seconds(); + // ephemeris.retain(|eph_i| { + // let toe = eph_i.toe_gpst(sv_ts).unwrap(); + // let dt = (t - toe).to_seconds(); + // if dt < max_dtoe { + // dt > 0.0 + // } else { + // false + // } + // }); + // !ephemeris.is_empty() + // } else { + // true + // } + //}); + output + } +} diff --git a/rinex-cli/src/positioning/interp/orbit.rs b/rinex-cli/src/positioning/orbit/sp3.rs similarity index 51% rename from rinex-cli/src/positioning/interp/orbit.rs rename to rinex-cli/src/positioning/orbit/sp3.rs index 1602c5c9e..0148f962e 100644 --- a/rinex-cli/src/positioning/interp/orbit.rs +++ b/rinex-cli/src/positioning/orbit/sp3.rs @@ -1,25 +1,26 @@ -use super::Buffer as BufferTrait; -use crate::cli::Context; +use crate::{cli::Context, positioning::BufferTrait}; use std::collections::HashMap; use gnss_rtk::prelude::{ - AprioriPosition, Arc, Bodies, Cosm, Epoch, Frame, - InterpolationResult as RTKInterpolationResult, LightTimeCalc, Vector3, SV, + Arc, Bodies, Cosm, Epoch, Frame, InterpolationResult as RTKInterpolationResult, LightTimeCalc, + TimeScale, Vector3, SV, }; -use rinex::{carrier::Carrier, navigation::Ephemeris}; +use rinex::carrier::Carrier; +#[derive(Debug)] struct Buffer { inner: Vec<(Epoch, (f64, f64, f64))>, } -impl BufferTrait for Buffer { +impl BufferTrait<(f64, f64, f64)> for Buffer { fn malloc(size: usize) -> Self { Self { inner: Vec::with_capacity(size), } } fn push(&mut self, x_j: (Epoch, (f64, f64, f64))) { + self.inner.retain(|k| *k != x_j); self.inner.push(x_j); } fn clear(&mut self) { @@ -45,20 +46,6 @@ impl BufferTrait for Buffer { } } -/// Orbital state interpolator -pub struct Interpolator<'a> { - // Interpolation order - order: usize, - // Total counter - epochs: usize, - // Reference position - apriori: AprioriPosition, - // Internal buffers - buffers: HashMap, - // Data source - iter: Box + 'a>, -} - /* * Evaluates Sun / Earth vector in meters ECEF at "t" */ @@ -72,85 +59,71 @@ fn sun_unit_vector(ref_frame: &Frame, cosmic: &Arc, t: Epoch) -> Vector3 Interpolator<'a> { - /* - * Orbit interpolator - * 1. Prefer SP3 product - * 2. BRDC last option - */ - pub fn from_ctx(ctx: &'a Context, order: usize, apriori: AprioriPosition) -> Self { +pub struct Orbit<'a> { + epochs: usize, + order: usize, + buffers: HashMap, + iter: Box + 'a>, +} + +impl<'a> Orbit<'a> { + pub fn from_ctx(ctx: &'a Context, order: usize) -> Self { let cosmic = Cosm::de438(); + let sp3 = ctx.data.sp3().unwrap(); let earth_frame = cosmic.frame("EME2000"); // this only works on planet Earth.. - assert!( - order % 2 > 0, - "only odd interpolation orders currently supported" - ); Self { order, - apriori, epochs: 0, buffers: HashMap::with_capacity(128), - iter: if let Some(sp3) = ctx.data.sp3() { - if let Some(atx) = ctx.data.antex() { - Box::new( - sp3.sv_position() - .filter_map(move |(t, sv, (x, y, z))| { - // TODO: need to complexify the whole interface - // to provide correct information with respect to frequency - if let Some(delta) = atx.sv_antenna_apc_offset(t, sv, Carrier::L1) { - let delta = Vector3::::new(delta.0, delta.1, delta.2); - let r_sat = - Vector3::::new(x * 1.0E3, y * 1.0E3, z * 1.0E3); - let k = -r_sat - / (r_sat[0].powi(2) + r_sat[1].powi(2) + r_sat[3].powi(2)) - .sqrt(); + iter: if let Some(atx) = ctx.data.antex() { + Box::new(sp3.sv_position().filter_map(move |(t, sv, (x, y, z))| { + // TODO: need to complexify the whole interface + // to provide correct information with respect to frequency + if let Some(delta) = atx.sv_antenna_apc_offset(t, sv, Carrier::L1) { + let delta = Vector3::::new(delta.0, delta.1, delta.2); + let r_sat = Vector3::::new(x * 1.0E3, y * 1.0E3, z * 1.0E3); + let k = -r_sat + / (r_sat[0].powi(2) + r_sat[1].powi(2) + r_sat[3].powi(2)).sqrt(); - let r_sun = sun_unit_vector(&earth_frame, &cosmic, t); - let norm = ((r_sun[0] - r_sat[0]).powi(2) - + (r_sun[1] - r_sat[1]).powi(2) - + (r_sun[2] - r_sat[2]).powi(2)) - .sqrt(); + let r_sun = sun_unit_vector(&earth_frame, &cosmic, t); + let norm = ((r_sun[0] - r_sat[0]).powi(2) + + (r_sun[1] - r_sat[1]).powi(2) + + (r_sun[2] - r_sat[2]).powi(2)) + .sqrt(); - let e = (r_sun - r_sat) / norm; - let j = - Vector3::::new(k[0] * e[0], k[1] * e[1], k[2] * e[2]); - let i = - Vector3::::new(j[0] * k[0], j[1] * k[1], j[2] * k[2]); - let r_dot = Vector3::::new( - (i[0] + j[0] + k[0]) * delta[0], - (i[1] + j[1] + k[1]) * delta[1], - (i[2] + j[2] + k[2]) * delta[2], - ); + let e = (r_sun - r_sat) / norm; + let j = Vector3::::new(k[0] * e[0], k[1] * e[1], k[2] * e[2]); + let i = Vector3::::new(j[0] * k[0], j[1] * k[1], j[2] * k[2]); + let r_dot = Vector3::::new( + (i[0] + j[0] + k[0]) * delta[0], + (i[1] + j[1] + k[1]) * delta[1], + (i[2] + j[2] + k[2]) * delta[2], + ); - let r_sat = r_sat + r_dot; - Some((t, sv, (r_sat[0], r_sat[1], r_sat[1]))) - } else { - error!("{:?} ({}) - failed to determine APC offset", t, sv); - None - } - }) - .peekable(), - ) - } else { - warn!("Cannot determine exact APC coordinates without ANTEX data."); - warn!("Expect tiny offsets in final results."); - Box::new( - sp3.sv_position() - .map(|(t, sv, (x, y, z))| (t, sv, (x * 1.0E3, y * 1.0E3, z * 1.0E3))), - ) - } + let r_sat = r_sat + r_dot; + Some((t, sv, (r_sat[0], r_sat[1], r_sat[1]))) + } else { + error!("{:?} ({}) - failed to determine APC offset", t, sv); + None + } + })) } else { - let brdc = ctx - .data - .brdc_navigation() - .expect("BRDC navigation required"); + warn!("Cannot determine exact APC coordinates without ANTEX data."); + warn!("Expect tiny offsets in final results."); Box::new( - brdc.sv_position() + sp3.sv_position() .map(|(t, sv, (x, y, z))| (t, sv, (x * 1.0E3, y * 1.0E3, z * 1.0E3))), ) }, } } + fn is_feasible(&self, t: Epoch, sv: SV) -> bool { + if let Some(buf) = self.buffers.get(&sv) { + buf.feasible(self.order, t) + } else { + false + } + } fn push(&mut self, t: Epoch, sv: SV, data: (f64, f64, f64)) { if let Some(buf) = self.buffers.get_mut(&sv) { buf.fill((t, data)); @@ -160,7 +133,6 @@ impl<'a> Interpolator<'a> { self.buffers.insert(sv, buf); } } - // Consumes N epochs completely fn consume(&mut self, total: usize) -> bool { let mut prev_t = None; let mut epochs = 0; @@ -170,40 +142,16 @@ impl<'a> Interpolator<'a> { if let Some(prev) = prev_t { if t > prev { epochs += 1; - //println!("{:?} - new epoch", t); //DEBUG } } prev_t = Some(t); } else { - //println!("consumed all data"); // DEBUG return true; } } self.epochs += epochs; false } - // fn latest(&self, sv: SV) -> Option<&Epoch> { - // self.buffers - // .iter() - // .filter_map(|(k, v)| { - // if *k == sv { - // let last = v.inner.iter().map(|(e, _)| e).last()?; - // Some(last) - // } else { - // None - // } - // }) - // .reduce(|k, _| k) - // } - // Returns true if interpolation is feasible @ t for SV - fn is_feasible(&self, t: Epoch, sv: SV) -> bool { - if let Some(buf) = self.buffers.get(&sv) { - buf.feasible(self.order, t) - } else { - false - } - } - // Orbit interpolation @ t for SV pub fn next_at(&mut self, t: Epoch, sv: SV) -> Option { // Maintain buffer up to date, consume data if need be while !self.is_feasible(t, sv) { @@ -214,81 +162,61 @@ impl<'a> Interpolator<'a> { } let buf = self.buffers.get_mut(&sv)?; - //let len_before = buf.len(); // DEBUG - let ref_ecef = self.apriori.ecef(); - if let Some((x, y, z)) = buf.direct_output(t) { // No need to interpolate @ t for SV // Preserves data precision - let el_az = - Ephemeris::elevation_azimuth((*x, *y, *z), (ref_ecef[0], ref_ecef[1], ref_ecef[2])); - return Some( - RTKInterpolationResult::from_apc_position((*x, *y, *z)) - .with_elevation_azimuth(el_az), - ); + return Some(RTKInterpolationResult::from_position((*x, *y, *z))); } let mut mid_offset = 0; let mut polynomials = (0.0_f64, 0.0_f64, 0.0_f64); let mut out = Option::::None; - for (index, (buf_t, _)) in buf.inner.iter().enumerate() { - if *buf_t > t { + for (index, (t_i, _)) in buf.inner.iter().enumerate() { + if *t_i > t { break; } mid_offset = index; } let (min_before, min_after) = ((self.order + 1) / 2, (self.order + 1) / 2); - //println!("t: {:?} | offset [{}] | snapshot {:?}", t, mid_offset, buf); //DEBUG - if out.is_none() { - // needs interpolation - if mid_offset >= min_before && buf.len() - mid_offset >= min_after { - let offset = mid_offset - (self.order + 1) / 2; - //println!("is feasible"); //DEBUG - for i in 0..=self.order { - let mut li = 1.0_f64; - let (t_i, (x_i, y_i, z_i)) = buf.inner[offset + i]; - for j in 0..=self.order { - let (t_j, _) = buf.inner[offset + j]; - if j != i { - li *= (t - t_j).to_seconds(); - li /= (t_i - t_j).to_seconds(); - } - } - polynomials.0 += x_i * li; - polynomials.1 += y_i * li; - polynomials.2 += z_i * li; + if mid_offset >= min_before && buf.len() - mid_offset >= min_after { + let offset = mid_offset - (self.order + 1) / 2; + for i in 0..=self.order { + let mut li = 1.0_f64; + + let (mut t_i, (x_i, y_i, z_i)) = buf.inner[offset + i]; + if t_i.time_scale != TimeScale::GPST { + t_i = Epoch::from_gpst_duration(t_i.to_gpst_duration()); } - let el_az = Ephemeris::elevation_azimuth( - polynomials, - (ref_ecef[0], ref_ecef[1], ref_ecef[2]), - ); - out = Some( - RTKInterpolationResult::from_apc_position(polynomials) - .with_elevation_azimuth(el_az), - ); - //} else { - // println!("not feasible"); //DEBUG + for j in 0..=self.order { + let (mut t_j, _) = buf.inner[offset + j]; + if t_j.time_scale != TimeScale::GPST { + t_j = Epoch::from_gpst_duration(t_j.to_gpst_duration()); + } + + if j != i { + li *= (t - t_j).to_seconds(); + li /= (t_i - t_j).to_seconds(); + } + } + polynomials.0 += x_i * li; + polynomials.1 += y_i * li; + polynomials.2 += z_i * li; } + out = Some(RTKInterpolationResult::from_position(polynomials)); } if out.is_some() { - // management: discard old samples - // len_before = buf.len(); // DEBUG - let index_min = mid_offset - (self.order + 1) / 2 - 2; - let mut index = 0; - buf.inner.retain(|_| { - index += 1; - index > index_min - }); - - //let len_after = buf.len(); // DEBUG - //if len_after != len_before { // DEBUG - // println!("{:?} - purge: t_min {:?} - snapshot {:?}", t, t_min, buf); //DEBUG - //} + // TODO improve memory footprint and avoid memory growth + //let index_min = mid_offset - (self.order + 1) / 2 - 2; + //let mut index = 0; + // buf.inner.retain(|_| { + // index += 1; + // index > index_min + // }); } out } diff --git a/rinex-cli/src/positioning/ppp/mod.rs b/rinex-cli/src/positioning/ppp/mod.rs index 089ece71f..5ba935bc0 100644 --- a/rinex-cli/src/positioning/ppp/mod.rs +++ b/rinex-cli/src/positioning/ppp/mod.rs @@ -1,14 +1,17 @@ //! PPP solver -use crate::cli::Context; -use crate::positioning::{ - bd_model, cast_rtk_carrier, interp::TimeInterpolator, kb_model, ng_model, tropo_components, +use crate::{ + cli::Context, + positioning::{ + bd_model, + cast_rtk_carrier, + kb_model, + ng_model, //tropo_components, + Time, + }, }; use std::collections::BTreeMap; -use rinex::{ - carrier::Carrier, - prelude::{Duration, SV}, -}; +use rinex::{carrier::Carrier, prelude::SV}; mod post_process; pub use post_process::{post_process, Error as PostProcessingError}; @@ -21,7 +24,7 @@ use rtk::prelude::{ pub fn resolve( ctx: &Context, mut solver: Solver, - rx_lat_ddeg: f64, + // rx_lat_ddeg: f64, ) -> BTreeMap where I: Fn(Epoch, SV, usize) -> Option, @@ -31,22 +34,9 @@ where // infaillible, at this point let obs_data = ctx.data.observation().unwrap(); let nav_data = ctx.data.brdc_navigation().unwrap(); + // let meteo_data = ctx.data.meteo(); //TODO - let clk_data = ctx.data.clock(); - let meteo_data = ctx.data.meteo(); - - let sp3_has_clock = ctx.data.sp3_has_clock(); - if clk_data.is_none() && sp3_has_clock { - if let Some(sp3) = ctx.data.sp3() { - warn!("Using clock states defined in SP3 file: CLK product should be prefered"); - if sp3.epoch_interval >= Duration::from_seconds(300.0) { - warn!("interpolating clock states from low sample rate SP3 will most likely introduce errors"); - } - } - } - - let mut interp = TimeInterpolator::from_ctx(ctx); - debug!("Clock interpolator created"); + let mut time = Time::from_ctx(ctx); for ((t, flag), (_clk, vehicles)) in obs_data.observation() { let mut candidates = Vec::::with_capacity(4); @@ -73,7 +63,7 @@ where // determine TOE let (_toe, sv_eph) = sv_eph.unwrap(); - let clock_corr = match interp.next_at(*t, *sv) { + let clock_corr = match time.next_at(*t, *sv) { Some(dt) => dt, None => { error!("{:?} ({}) - failed to determine clock correction", *t, *sv); @@ -124,7 +114,7 @@ where } // grab possible tropo components - let zwd_zdd = tropo_components(meteo_data, *t, rx_lat_ddeg); + // let zwd_zdd = tropo_components(meteo_data, *t, rx_lat_ddeg); let iono_bias = IonosphereBias { kb_model: kb_model(nav_data, *t), @@ -134,8 +124,8 @@ where }; let tropo_bias = TroposphereBias { - total: None, //TODO - zwd_zdd, + total: None, //TODO + zwd_zdd: None, //TODO }; match solver.resolve(*t, &candidates, &iono_bias, &tropo_bias) { diff --git a/rinex-cli/src/positioning/ppp/post_process.rs b/rinex-cli/src/positioning/ppp/post_process.rs index fd8d2f553..03fb17ae7 100644 --- a/rinex-cli/src/positioning/ppp/post_process.rs +++ b/rinex-cli/src/positioning/ppp/post_process.rs @@ -1,8 +1,16 @@ -use crate::cli::Context; +use std::{ + collections::{BTreeMap, HashMap}, + fs::File, + io::Write, +}; + +use crate::{ + cli::Context, + fops::open_with_web_browser, + graph::{build_3d_chart_epoch_label, build_chart_epoch_axis, PlotContext}, +}; use clap::ArgMatches; -use std::collections::{BTreeMap, HashMap}; -use std::fs::File; -use std::io::Write; + use thiserror::Error; use hifitime::Epoch; @@ -19,15 +27,14 @@ use kml::{ extern crate geo_types; use geo_types::Point as GeoPoint; -use plotly::color::NamedColor; -use plotly::common::Mode; -use plotly::common::{Marker, MarkerSymbol}; -use plotly::layout::MapboxStyle; -use plotly::ScatterMapbox; +use plotly::{ + color::NamedColor, + common::{Marker, MarkerSymbol, Mode, Visible}, + layout::MapboxStyle, + ScatterMapbox, +}; -use crate::fops::open_with_web_browser; -use crate::graph::{build_3d_chart_epoch_label, build_chart_epoch_axis, PlotContext}; -use map_3d::{ecef2geodetic, rad2deg, Ellipsoid}; +use map_3d::{deg2rad, ecef2geodetic, rad2deg, Ellipsoid}; #[derive(Debug, Error)] pub enum Error { @@ -39,67 +46,33 @@ pub enum Error { KmlError(#[from] kml::Error), } -pub fn post_process( - ctx: &Context, - results: BTreeMap, - matches: &ArgMatches, -) -> Result<(), Error> { - // create a dedicated plot context - let mut plot_ctx = PlotContext::new(); - - let (x, y, z) = ctx.rx_ecef.unwrap(); // cannot fail at this point - - let (lat_rad, lon_rad, _) = ecef2geodetic(x, y, z, Ellipsoid::WGS84); - let lat_ddeg = rad2deg(lat_rad); - let lon_ddeg = rad2deg(lon_rad); - - let epochs = results.keys().copied().collect::>(); - - let (mut lat, mut lon) = (Vec::::new(), Vec::::new()); - for result in results.values() { - let px = x + result.position.x; - let py = y + result.position.y; - let pz = z + result.position.z; - let (lat_ddeg, lon_ddeg, _) = ecef2geodetic(px, py, pz, Ellipsoid::WGS84); - lat.push(rad2deg(lat_ddeg)); - lon.push(rad2deg(lon_ddeg)); - } - - plot_ctx.add_world_map( - "PVT solutions", - true, // show legend - MapboxStyle::OpenStreetMap, - (lat_ddeg, lon_ddeg), //center - 18, // zoom in!! - ); - - let ref_scatter = ScatterMapbox::new(vec![lat_ddeg], vec![lon_ddeg]) - .marker( - Marker::new() - .size(5) - .symbol(MarkerSymbol::Circle) - .color(NamedColor::Red), - ) - .name("Apriori"); - plot_ctx.add_trace(ref_scatter); - - let pvt_scatter = ScatterMapbox::new(lat, lon) - .marker( - Marker::new() - .size(5) - .symbol(MarkerSymbol::Circle) - .color(NamedColor::Black), - ) - .name("PVT"); - plot_ctx.add_trace(pvt_scatter); - +// Customize HTML plot in case _apriori_ is known ahead of time +fn html_add_apriori_position( + epochs: &Vec, + solutions: &BTreeMap, + apriori_ecef: (f64, f64, f64), + plot_ctx: &mut PlotContext, +) { + let (x0, y0, z0) = apriori_ecef; + //let (lat0_rad, lon0_rad, _) = ecef2geodetic(x0, y0, z0, Ellipsoid::WGS84); + + // |x-x*|, |y-y*|, |z-z*| 3D comparison let trace = build_3d_chart_epoch_label( "error", Mode::Markers, epochs.clone(), - results.values().map(|e| e.position.x).collect::>(), - results.values().map(|e| e.position.y).collect::>(), - results.values().map(|e| e.position.z).collect::>(), + solutions + .values() + .map(|pvt| pvt.position.x - x0) + .collect::>(), + solutions + .values() + .map(|pvt| pvt.position.y - y0) + .collect::>(), + solutions + .values() + .map(|pvt| pvt.position.z - z0) + .collect::>(), ); plot_ctx.add_cartesian3d_plot( @@ -110,29 +83,17 @@ pub fn post_process( ); plot_ctx.add_trace(trace); - let mut worst_radius = 1000.0_f64; - /* - * Add Spherical mesh with radius being the - * largest error - */ - for error in results.values().map(|pvt| { - (pvt.position.x.powi(2) + pvt.position.y.powi(2) + pvt.position.z.powi(2)).sqrt() - }) { - if error > worst_radius { - worst_radius = error; - } - } - - /* - * 2D X/Y errors graph - */ + // 2D |x-x*|, |y-y*| comparison plot_ctx.add_timedomain_2y_plot("X / Y Errors", "X error [m]", "Y error [m]"); let trace = build_chart_epoch_axis( "x err", Mode::Markers, epochs.clone(), - results.values().map(|p| p.position.x).collect::>(), + solutions + .values() + .map(|pvt| pvt.position.x - x0) + .collect::>(), ); plot_ctx.add_trace(trace); @@ -140,36 +101,154 @@ pub fn post_process( "y err", Mode::Markers, epochs.clone(), - results.values().map(|p| p.position.y).collect::>(), + solutions + .values() + .map(|pvt| pvt.position.y - y0) + .collect::>(), ) .y_axis("y2"); plot_ctx.add_trace(trace); - /* - * 2D Z error graph - */ + // |z-z*| comparison plot_ctx.add_timedomain_plot("Altitude Errors", "Z error [m]"); let trace = build_chart_epoch_axis( "z err", Mode::Markers, epochs.clone(), - results.values().map(|p| p.position.z).collect::>(), + solutions + .values() + .map(|pvt| pvt.position.z - z0) + .collect::>(), ); plot_ctx.add_trace(trace); +} - /* - * Create graphical visualization - * vel(x), vel(yy : dual plot - * vel(z) : dedicated plot - * hdop, vdop : dual plot - * dt, tdop : dual plot - */ +pub fn post_process( + ctx: &Context, + solutions: BTreeMap, + matches: &ArgMatches, +) -> Result<(), Error> { + // create a dedicated plot context + let mut plot_ctx = PlotContext::new(); + + // Convert solutions to geodetic DDEG + let (mut lat, mut lon) = (Vec::::new(), Vec::::new()); + for solution in solutions.values() { + let (lat_rad, lon_rad, _) = ecef2geodetic( + solution.position.x, + solution.position.y, + solution.position.z, + Ellipsoid::WGS84, + ); + lat.push(rad2deg(lat_rad)); + lon.push(rad2deg(lon_rad)); + } + + let nb_solutions = solutions.len(); + let final_solution_ddeg = (lat[nb_solutions - 1], lon[nb_solutions - 1]); + let epochs = solutions.keys().copied().collect::>(); + + let lat0_rad = if let Some(apriori_ecef) = ctx.rx_ecef { + ecef2geodetic( + apriori_ecef.0, + apriori_ecef.1, + apriori_ecef.2, + Ellipsoid::WGS84, + ) + .0 + } else { + deg2rad(final_solution_ddeg.0) + }; + + let lon0_rad = if let Some(apriori_ecef) = ctx.rx_ecef { + ecef2geodetic( + apriori_ecef.0, + apriori_ecef.1, + apriori_ecef.2, + Ellipsoid::WGS84, + ) + .1 + } else { + deg2rad(final_solution_ddeg.1) + }; + + // TODO: would be nice to adapt zoom level + plot_ctx.add_world_map( + "PVT solutions", + true, // show legend + MapboxStyle::OpenStreetMap, + final_solution_ddeg, //center + 18, // zoom + ); + + // Mark _apriori_ position, if it exists + if let Some(apriori_ecef) = ctx.rx_ecef { + let (lat0_rad, lon0_rad, _) = ecef2geodetic( + apriori_ecef.0, + apriori_ecef.1, + apriori_ecef.2, + Ellipsoid::WGS84, + ); + let (lat0_ddeg, lon0_ddeg) = (rad2deg(lat0_rad), rad2deg(lon0_rad)); + let apriori_scatter = ScatterMapbox::new(vec![lat0_ddeg], vec![lon0_ddeg]) + .marker( + Marker::new() + .size(5) + .symbol(MarkerSymbol::Circle) + .color(NamedColor::Red), + ) + .name("Apriori"); + plot_ctx.add_trace(apriori_scatter); + } + + // Process solutions + // Scatter Mapbox (map projection) + let mut prev_pct = 0; + for (index, (_, sol_i)) in solutions.iter().enumerate() { + let (lat_rad, lon_rad, _) = ecef2geodetic( + sol_i.position.x, + sol_i.position.y, + sol_i.position.z, + Ellipsoid::WGS84, + ); + let (lat_ddeg, lon_ddeg) = (rad2deg(lat_rad), rad2deg(lon_rad)); + + let pct = index * 100 / nb_solutions; + if pct % 10 == 0 && index > 0 && pct != prev_pct || index == nb_solutions - 1 { + let (title, visible, symbol, color) = if index == nb_solutions - 1 { + ( + "FINAL".to_string(), + Visible::True, + MarkerSymbol::Circle, + NamedColor::Black, + ) + } else { + ( + format!("Solver: {:02}%", pct), + Visible::LegendOnly, + MarkerSymbol::Circle, + NamedColor::Black, + ) + }; + let pvt_scatter = ScatterMapbox::new(vec![lat_ddeg], vec![lon_ddeg]) + .marker(Marker::new().size(5).color(color).symbol(symbol)) + .visible(visible) + .name(&title); + plot_ctx.add_trace(pvt_scatter); + } + prev_pct = pct; + } + + // Absolute velocity plot_ctx.add_timedomain_2y_plot("Velocity (X & Y)", "Speed [m/s]", "Speed [m/s]"); let trace = build_chart_epoch_axis( "velocity (x)", Mode::Markers, epochs.clone(), - results.values().map(|p| p.velocity.x).collect::>(), + solutions + .values() + .map(|pvt| pvt.velocity.x) + .collect::>(), ); plot_ctx.add_trace(trace); @@ -177,7 +256,10 @@ pub fn post_process( "velocity (y)", Mode::Markers, epochs.clone(), - results.values().map(|p| p.velocity.y).collect::>(), + solutions + .values() + .map(|pvt| pvt.velocity.y) + .collect::>(), ) .y_axis("y2"); plot_ctx.add_trace(trace); @@ -187,75 +269,83 @@ pub fn post_process( "velocity (z)", Mode::Markers, epochs.clone(), - results.values().map(|p| p.velocity.z).collect::>(), + solutions + .values() + .map(|pvt| pvt.velocity.z) + .collect::>(), ); plot_ctx.add_trace(trace); - plot_ctx.add_timedomain_plot("GDOP", "GDOP [m]"); + plot_ctx.add_timedomain_2y_plot("Clock offset", "dt [s]", "TDOP [s]"); let trace = build_chart_epoch_axis( - "gdop", + "dt", Mode::Markers, epochs.clone(), - results.values().map(|e| e.gdop).collect::>(), + solutions + .values() + .map(|pvt| pvt.dt.to_seconds()) + .collect::>(), ); plot_ctx.add_trace(trace); - plot_ctx.add_timedomain_2y_plot("HDOP, VDOP", "HDOP [m]", "VDOP [m]"); let trace = build_chart_epoch_axis( - "hdop", + "tdop", Mode::Markers, epochs.clone(), - results - .values() - .map(|e| e.hdop(lat_ddeg, lon_ddeg)) - .collect::>(), - ); + solutions.values().map(|pvt| pvt.tdop).collect::>(), + ) + .y_axis("y2"); plot_ctx.add_trace(trace); + // Dilution of Precision + plot_ctx.add_timedomain_plot("GDOP", "GDOP [m]"); let trace = build_chart_epoch_axis( - "vdop", + "gdop", Mode::Markers, epochs.clone(), - results - .values() - .map(|e| e.vdop(lat_ddeg, lon_ddeg)) - .collect::>(), - ) - .y_axis("y2"); + solutions.values().map(|pvt| pvt.gdop).collect::>(), + ); plot_ctx.add_trace(trace); - plot_ctx.add_timedomain_2y_plot("Clock offset", "dt [s]", "TDOP [s]"); + plot_ctx.add_timedomain_2y_plot("HDOP, VDOP", "HDOP [m]", "VDOP [m]"); let trace = build_chart_epoch_axis( - "dt", + "hdop", Mode::Markers, epochs.clone(), - results + solutions .values() - .map(|e| e.dt.to_seconds()) + .map(|pvt| pvt.hdop(lat0_rad, lon0_rad)) .collect::>(), ); plot_ctx.add_trace(trace); let trace = build_chart_epoch_axis( - "tdop", + "vdop", Mode::Markers, epochs.clone(), - results.values().map(|e| e.tdop).collect::>(), + solutions + .values() + .map(|pvt| pvt.vdop(lat0_rad, lon0_rad)) + .collect::>(), ) .y_axis("y2"); plot_ctx.add_trace(trace); + if let Some(apriori_ecef) = ctx.rx_ecef { + html_add_apriori_position(&epochs, &solutions, apriori_ecef, &mut plot_ctx); + } + // render plots - let graphs = ctx.workspace.join("PPP.html"); + let graphs = ctx.workspace.join("Solutions.html"); let graphs = graphs.to_string_lossy().to_string(); let mut fd = File::create(&graphs).unwrap_or_else(|_| panic!("failed to crate \"{}\"", graphs)); - write!(fd, "{}", plot_ctx.to_html()).expect("failed to render rtk visualization"); + write!(fd, "{}", plot_ctx.to_html()).expect("failed to render PVT solutions"); info!("\"{}\" solutions generated", graphs); /* * Generate txt, GPX, KML.. */ - let txtpath = ctx.workspace.join("PVT.csv"); + let txtpath = ctx.workspace.join("solutions.csv"); let txtfile = txtpath.to_string_lossy().to_string(); let mut fd = File::create(&txtfile)?; @@ -264,31 +354,29 @@ pub fn post_process( writeln!( fd, - "Epoch, dx, dy, dz, x_ecef, y_ecef, z_ecef, speed_x, speed_y, speed_z, hdop, vdop, rcvr_clock_bias, tdop" + "Epoch, x_ecef, y_ecef, z_ecef, speed_x, speed_y, speed_z, hdop, vdop, rcvr_clock_bias, tdop" )?; - for (epoch, solution) in results { - let (px, py, pz) = ( - x + solution.position.x, - y + solution.position.y, - z + solution.position.z, + for (epoch, solution) in solutions { + let (lat_rad, lon_rad, alt) = ecef2geodetic( + solution.position.x, + solution.position.y, + solution.position.z, + Ellipsoid::WGS84, ); - let (lat, lon, alt) = map_3d::ecef2geodetic(px, py, pz, Ellipsoid::WGS84); + let (lat_ddeg, lon_ddeg) = (rad2deg(lat_rad), rad2deg(lon_rad)); let (hdop, vdop, tdop) = ( - solution.hdop(lat_ddeg, lon_ddeg), - solution.vdop(lat_ddeg, lon_ddeg), + solution.hdop(lat_rad, lon_rad), + solution.vdop(lat_rad, lon_rad), solution.tdop, ); writeln!( fd, - "{:?}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}", + "{:?}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}, {:.6E}", epoch, solution.position.x, solution.position.y, solution.position.z, - px, - py, - pz, solution.velocity.x, solution.velocity.y, solution.velocity.z, @@ -299,14 +387,14 @@ pub fn post_process( )?; if matches.get_flag("gpx") { let mut segment = gpx::TrackSegment::new(); - let mut wp = Waypoint::new(GeoPoint::new(rad2deg(lon), rad2deg(lat))); // Yes, longitude *then* latitude + let mut wp = Waypoint::new(GeoPoint::new(lon_ddeg, lat_ddeg)); // Yes, longitude *then* latitude wp.elevation = Some(alt); - wp.speed = None; // TODO ? + wp.speed = None; // TODO wp.time = None; // TODO Gpx::Time wp.name = Some(format!("{:?}", epoch)); wp.hdop = Some(hdop); wp.vdop = Some(vdop); - wp.sat = None; //TODO: nb of contributing satellites + wp.sat = None; //TODO: nb of satellites wp.dgps_age = None; //TODO: Number of seconds since last DGPS update, from the element. wp.dgpsid = None; //TODO: ID of DGPS station used in differential correction, in the range [0, 1023]. segment.points.push(wp); @@ -320,8 +408,8 @@ pub fn post_process( Some(KmlGeometry::Point(KmlPoint { coord: { KmlCoord { - x: rad2deg(lat), - y: rad2deg(lon), + x: lat_ddeg, + y: lon_ddeg, z: Some(alt), } }, @@ -364,7 +452,7 @@ pub fn post_process( let kmldoc = KmlDocument { version: KmlVersion::V23, attrs: [( - String::from("rtk-version"), + String::from("https://georust.org/"), env!("CARGO_PKG_VERSION").to_string(), )] .into_iter() @@ -382,7 +470,7 @@ pub fn post_process( } if !ctx.quiet { - let graphs = ctx.workspace.join("PPP.html"); + let graphs = ctx.workspace.join("Solutions.html"); let graphs = graphs.to_string_lossy().to_string(); open_with_web_browser(&graphs); } diff --git a/rinex-cli/src/positioning/interp/time.rs b/rinex-cli/src/positioning/time/interp.rs similarity index 58% rename from rinex-cli/src/positioning/interp/time.rs rename to rinex-cli/src/positioning/time/interp.rs index 1b08887af..e31e977fa 100644 --- a/rinex-cli/src/positioning/interp/time.rs +++ b/rinex-cli/src/positioning/time/interp.rs @@ -1,32 +1,31 @@ -use super::Buffer as BufferTrait; -use crate::cli::Context; +use crate::positioning::BufferTrait; use gnss_rtk::prelude::{Duration, Epoch, SV}; use std::collections::HashMap; #[derive(Debug)] struct Buffer { - inner: Vec<(Epoch, (f64, f64, f64))>, + inner: Vec<(Epoch, f64)>, } -impl BufferTrait for Buffer { +impl BufferTrait for Buffer { fn malloc(size: usize) -> Self { Self { inner: Vec::with_capacity(size), } } - fn push(&mut self, x_j: (Epoch, (f64, f64, f64))) { + fn push(&mut self, x_j: (Epoch, f64)) { self.inner.push(x_j); } fn clear(&mut self) { self.inner.clear(); } - fn snapshot(&self) -> &[(Epoch, (f64, f64, f64))] { + fn snapshot(&self) -> &[(Epoch, f64)] { &self.inner } - fn snapshot_mut(&mut self) -> &mut [(Epoch, (f64, f64, f64))] { + fn snapshot_mut(&mut self) -> &mut [(Epoch, f64)] { &mut self.inner } - fn get(&self, index: usize) -> Option<&(Epoch, (f64, f64, f64))> { + fn get(&self, index: usize) -> Option<&(Epoch, f64)> { self.inner.get(index) } fn len(&self) -> usize { @@ -49,52 +48,18 @@ pub struct Interpolator<'a> { epochs: usize, /// Internal buffer buffers: HashMap, - iter: Box + 'a>, + iter: Box + 'a>, } impl<'a> Interpolator<'a> { - /* - * Time interpolator - * 1. Prefer CLK product - * 2. Prefer SP3 product - * 3. BRDC last option - */ - pub fn from_ctx(ctx: &'a Context) -> Self { + pub fn from_iter(iter: impl Iterator + 'a) -> Self { Self { epochs: 0, + iter: Box::new(iter), buffers: HashMap::with_capacity(32), - iter: if let Some(clk) = ctx.data.clock() { - Box::new(clk.precise_sv_clock().map(|(t, sv, _, prof)| { - ( - t, - sv, - ( - prof.bias, - prof.drift.unwrap_or(0.0_f64), - prof.drift_change.unwrap_or(0.0_f64), - ), - ) - })) - } else if let Some(sp3) = ctx.data.sp3() { - // TODO: improve SP3 API and definitions - Box::new( - sp3.sv_clock() - .map(|(t, sv, clk)| (t, sv, (clk, 0.0_f64, 0.0_f64))), - ) - } else { - panic!("SP3 or CLOCK RINEX currently required"); - // TODO - // let brdc = ctx.data.brdc_navigation().unwrap(); // infaillible - // Box::new(brdc.sv_clock()) - // dt = t - toc - // for (i=0; i<2; i++) - // dt -= a0 + a1 * dt+ a2 * dt^2 - // return a0 + a1 * dt + a2 * dt - // let clock_corr = Ephemeris::sv_clock_corr(*sv, clock_state, *t, toe); - }, } } - fn push(&mut self, t: Epoch, sv: SV, data: (f64, f64, f64)) { + fn push(&mut self, t: Epoch, sv: SV, data: f64) { if let Some(buf) = self.buffers.get_mut(&sv) { buf.push((t, data)); } else { @@ -103,7 +68,6 @@ impl<'a> Interpolator<'a> { self.buffers.insert(sv, buf); } } - // consumes N epochs completely fn consume(&mut self, total: usize) -> bool { let mut prev_t = None; let mut epochs = 0; @@ -125,19 +89,6 @@ impl<'a> Interpolator<'a> { self.epochs += epochs; false } - // fn latest(&self, sv: SV) -> Option<&Epoch> { - // self.buffers - // .iter() - // .filter_map(|(k, v)| { - // if *k == sv { - // let last = v.inner.iter().map(|(e, _)| e).last()?; - // Some(last) - // } else { - // None - // } - // }) - // .reduce(|k, _| k) - // } // Returns true if interpolation is feasible @ t for SV fn is_feasible(&self, t: Epoch, sv: SV) -> bool { if let Some(buf) = self.buffers.get(&sv) { @@ -161,17 +112,17 @@ impl<'a> Interpolator<'a> { let buf = self.buffers.get_mut(&sv)?; - if let Some((y, _, _)) = buf.direct_output(t) { + if let Some(y) = buf.direct_output(t) { // No need to interpolate @ t for SV // Preserves data precision first_x = Some(t); dt = Some(Duration::from_seconds(*y)); - } else if let Some((before_x, (before_y, _, _))) = + } else if let Some((before_x, before_y)) = buf.inner.iter().filter(|(v_t, _)| *v_t < t).last() { first_x = Some(*before_x); - if let Some((after_x, (after_y, _, _))) = buf + if let Some((after_x, after_y)) = buf .inner .iter() .filter(|(v_t, _)| *v_t >= t) diff --git a/rinex-cli/src/positioning/time/mod.rs b/rinex-cli/src/positioning/time/mod.rs new file mode 100644 index 000000000..fdba2e061 --- /dev/null +++ b/rinex-cli/src/positioning/time/mod.rs @@ -0,0 +1,47 @@ +use crate::cli::Context; +use gnss_rtk::prelude::{Duration, Epoch, SV}; + +mod interp; +use interp::Interpolator; + +mod nav; +use nav::Time as NAVTime; + +pub enum Time<'a> { + NAV(NAVTime<'a>), + Interp(Interpolator<'a>), +} + +impl<'a> Time<'a> { + /* + * Time source + * 1. Prefer CLK product + * 2. Prefer SP3 product + * 3. BRDC last option + */ + pub fn from_ctx(ctx: &'a Context) -> Self { + if let Some(clk) = ctx.data.clock() { + let iter = Box::new( + clk.precise_sv_clock() + .map(|(t, sv, _, prof)| (t, sv, prof.bias)), + ); + Self::Interp(Interpolator::from_iter(iter)) + } else { + if ctx.data.sp3_has_clock() { + let sp3 = ctx.data.sp3().unwrap(); + let iter = sp3.sv_clock(); + Self::Interp(Interpolator::from_iter(iter)) + } else { + let brdc = ctx.data.brdc_navigation().unwrap(); // infaillible + let iter = brdc.ephemeris().map(|(_, (_, sv, eph))| (sv, eph)); + Self::NAV(NAVTime::from_iter(iter)) + } + } + } + pub fn next_at(&mut self, t: Epoch, sv: SV) -> Option { + match self { + Self::NAV(nav) => nav.next_at(t, sv), + Self::Interp(interp) => interp.next_at(t, sv), + } + } +} diff --git a/rinex-cli/src/positioning/time/nav.rs b/rinex-cli/src/positioning/time/nav.rs new file mode 100644 index 000000000..2ecf6d19e --- /dev/null +++ b/rinex-cli/src/positioning/time/nav.rs @@ -0,0 +1,95 @@ +use gnss_rtk::prelude::{Duration, Epoch, TimeScale, SV}; +use rinex::navigation::Ephemeris; +use std::collections::HashMap; + +pub struct Time<'a> { + buffer: HashMap>, + iter: Box + 'a>, +} + +impl<'a> Time<'a> { + pub fn from_iter(iter: impl Iterator + 'a) -> Self { + Self { + iter: Box::new(iter), + buffer: HashMap::with_capacity(32), + } + } + fn feasible(&self, t: Epoch, sv: SV, sv_ts: TimeScale) -> bool { + let max_dtoe = Ephemeris::max_dtoe(sv.constellation).unwrap(); + if let Some(dataset) = self.buffer.get(&sv) { + let mut index = dataset.len(); + while index > 1 { + index -= 1; + let eph_i = &dataset[index]; + if let Some(toe) = eph_i.toe_gpst(sv_ts) { + if toe < t && (t - toe) < max_dtoe { + return true; + } + } + } + false + } else { + false + } + } + pub fn next_at(&mut self, t: Epoch, sv: SV) -> Option { + let sv_ts = sv.timescale()?; + + while !self.feasible(t, sv, sv_ts) { + if let Some((sv_i, eph_i)) = self.iter.next() { + if let Some(dataset) = self.buffer.get_mut(&sv_i) { + dataset.push(eph_i.clone()); + } else { + self.buffer.insert(sv_i, vec![eph_i.clone()]); + } + } else { + // EOF + return None; + } + } + + let output = match self.buffer.get(&sv) { + None => None, + Some(ephemeris) => { + let eph_i = ephemeris.iter().min_by_key(|eph_i| { + let toe_i = eph_i.toe_gpst(sv_ts).unwrap(); + t - toe_i + })?; + + let (a0, a1, a2) = (eph_i.clock_bias, eph_i.clock_drift, eph_i.clock_drift_rate); + let t_gpst = t.to_time_scale(TimeScale::GPST).duration.to_seconds(); + let toe_gpst = eph_i.toe_gpst(sv_ts)?.duration.to_seconds(); + + let mut dt = t_gpst - toe_gpst; + if dt > 302400.0 { + dt -= 604800.0; + } else if dt < -302400.0 { + dt += 604800.0; + } + Some(Duration::from_seconds(a0 + a1 * dt + a2 * dt.powi(2))) + }, + }; + + // TODO improve memory footprint: avoid memory growth + //self.buffer.retain(|sv_i, ephemeris| { + // if *sv_i == sv { + // let max_dtoe = Ephemeris::max_dtoe(sv.constellation) + // .unwrap() + // .to_seconds(); + // ephemeris.retain(|eph_i| { + // let toe = eph_i.toe_gpst(sv_ts).unwrap(); + // let dt = (t - toe).to_seconds(); + // if dt < max_dtoe { + // dt > 0.0 + // } else { + // false + // } + // }); + // !ephemeris.is_empty() + // } else { + // true + // } + //}); + output + } +} diff --git a/rinex-qc/Cargo.toml b/rinex-qc/Cargo.toml index 3be9c031e..65e9ab216 100644 --- a/rinex-qc/Cargo.toml +++ b/rinex-qc/Cargo.toml @@ -20,7 +20,7 @@ rustdoc-args = ["--cfg", "docrs", "--generate-link-to-definition"] [dependencies] serde = { version = "1.0", optional = true, default-features = false, features = ["derive"] } -hifitime = "3.9.0" +hifitime = { git = "https://github.com/nyx-space/hifitime.git", branch = "master" } strum = "0.26" strum_macros = "0.26" horrorshow = "0.8" @@ -29,7 +29,9 @@ statrs = "0.16" sp3 = { path = "../sp3", version = "=1.0.8", features = ["serde"] } rinex-qc-traits = { path = "../qc-traits", version = "=0.1.1" } rinex = { path = "../rinex", version = "=0.16.1", features = ["full"] } -gnss-rs = { version = "2.1.3", features = ["serde"] } + +# gnss-rs = { version = "2.1.3", features = ["serde"] } +gnss-rs = { git = "https://github.com/rtk-rs/gnss", branch = "main", features = ["serde"] } [dev-dependencies] serde_json = "1" diff --git a/rinex/Cargo.toml b/rinex/Cargo.toml index 02e2ed443..778dd60b4 100644 --- a/rinex/Cargo.toml +++ b/rinex/Cargo.toml @@ -97,9 +97,11 @@ geo = { version = "0.28", optional = true } wkt = { version = "0.10.0", default-features = false, optional = true } serde = { version = "1.0", optional = true, default-features = false, features = ["derive"] } flate2 = { version = "1.0.24", optional = true, default-features = false, features = ["zlib"] } -hifitime = { version = "3.9.0", features = ["serde", "std"] } +hifitime = { git = "https://github.com/nyx-space/hifitime.git", branch = "master", features = ["serde", "std"] } horrorshow = { version = "0.8", optional = true } -gnss-rs = { version = "2.1.3", features = ["serde"] } + +# gnss-rs = { version = "2.1.3", features = ["serde"] } +gnss-rs = { git = "https://github.com/rtk-rs/gnss", branch = "main", features = ["serde"] } # RINEX QC dedicated traits rinex-qc-traits = { path = "../qc-traits", version = "=0.1.1", optional = true } diff --git a/rinex/src/algorithm/filters/smoothing.rs b/rinex/src/algorithm/filters/smoothing.rs index 2cd1d5ef7..abf9d475c 100644 --- a/rinex/src/algorithm/filters/smoothing.rs +++ b/rinex/src/algorithm/filters/smoothing.rs @@ -1,4 +1,5 @@ use crate::{preprocessing::TargetItem, Duration}; +use hifitime::EpochError; use thiserror::Error; /// Supported Smoothing Filters @@ -29,7 +30,7 @@ pub enum Error { #[error("invalid target")] InvalidTarget(#[from] crate::algorithm::target::Error), #[error("failed to parse duration")] - DurationParsing(#[from] hifitime::Errors), + DurationParsing(#[from] EpochError), } impl std::str::FromStr for SmoothingFilter { diff --git a/rinex/src/algorithm/target.rs b/rinex/src/algorithm/target.rs index 99d7d4078..58b64ca27 100644 --- a/rinex/src/algorithm/target.rs +++ b/rinex/src/algorithm/target.rs @@ -3,6 +3,7 @@ use crate::navigation::{orbits::NAV_ORBITS, FrameClass, NavMsgType}; use crate::observable; use crate::observable::Observable; use crate::prelude::*; +use hifitime::ParsingError as EpochParsingError; use std::str::FromStr; use thiserror::Error; @@ -37,7 +38,7 @@ pub enum Error { #[error("observable parsing error")] ObservableParsing(#[from] observable::ParsingError), #[error("invalid duration description")] - InvalidDurationItem(#[from] hifitime::Errors), + InvalidDurationItem(#[from] EpochParsingError), } /// Target Item represents items that filters diff --git a/rinex/src/carrier.rs b/rinex/src/carrier.rs index 62581c00e..6a64be72c 100644 --- a/rinex/src/carrier.rs +++ b/rinex/src/carrier.rs @@ -35,23 +35,23 @@ pub enum Carrier { G2a, /// Glonass channel 3 G3, - /// E1: GAL + /// E1 (Galileo) E1, - /// E5: GAL (E5a + E5b) + /// E5 (Galileo) E5, - /// E5a: GAL E5a + /// E5a (Galileo) E5a, - /// E5b: GAL E5b + /// E5b (Galileo) E5b, - /// E6: GAL military + /// E6 (Galileo military) E6, - /// B1: BeiDou 1i + /// B1 (BDS) B1I, - /// B1A BeiDou 1A + /// B1A (BDS) B1A, - /// B1C BeiDou 1C + /// B1C (BDS) B1C, - /// B2: BeiDou 2 + /// B2 (BDS) B2, /// B2i: BeiDou 2i B2I, @@ -186,16 +186,12 @@ impl Carrier { } pub fn frequency_mhz(&self) -> f64 { match self { - /* - * GPS, Gal, QZSS, SBAS - */ - Self::L1 | Self::E1 => 1575.42_f64, + Self::L1 | Self::E1 | Self::B1A | Self::B1C => 1575.42_f64, Self::L2 => 1227.60_f64, Self::L6 | Self::E6 => 1278.750_f64, - Self::L5 => 1176.45_f64, - Self::E5 => 1191.795_f64, - Self::E5a => 1176.45_f64, - Self::E5b => 1207.140_f64, + Self::L5 | Self::E5a | Self::B2A => 1176.45_f64, + Self::E5 | Self::B2 => 1191.795_f64, + Self::E5b | Self::B2I | Self::B2B => 1207.140_f64, /* * IRNSS */ @@ -214,10 +210,6 @@ impl Carrier { * BeiDou */ Self::B1I => 1561.098_f64, - Self::B1C | Self::B1A => 1575.420_f64, - Self::B2A => 1176.450_f64, - Self::B2I | Self::B2B => 1207.140_f64, - Self::B2 => 1191.795_f64, Self::B3 | Self::B3A => 1268.520_f64, /* * DORIS @@ -325,20 +317,20 @@ impl Carrier { other => *other, } } - pub(crate) fn gpsl1_codes() -> [&'static str; 39] { + pub(crate) fn gpsl1_codes() -> [&'static str; 40] { [ - "C1", "L1", "D1", "S1", "C1C", "L1C", "D1C", "S1C", "C1S", "L1S", "D1S", "S1S", "C1L", - "L1L", "D1L", "S1L", "C1X", "L1X", "D1X", "S1X", "C1P", "L1P", "D1P", "S1P", "C1W", - "L1W", "D1W", "S1W", "C1Y", "L1Y", "D1Y", "S1Y", "C1M", "L1M", "D1M", "S1M", "L1N", - "D1N", "S1N", + "C1", "L1", "D1", "S1", "P1", "C1C", "L1C", "D1C", "S1C", "C1S", "L1S", "D1S", "S1S", + "C1L", "L1L", "D1L", "S1L", "C1X", "L1X", "D1X", "S1X", "C1P", "L1P", "D1P", "S1P", + "C1W", "L1W", "D1W", "S1W", "C1Y", "L1Y", "D1Y", "S1Y", "C1M", "L1M", "D1M", "S1M", + "L1N", "D1N", "S1N", ] } - pub(crate) fn gpsl2_codes() -> [&'static str; 43] { + pub(crate) fn gpsl2_codes() -> [&'static str; 44] { [ - "C2", "L2", "D2", "S2", "C2C", "L2C", "D2C", "S2C", "C2D", "L2D", "D2D", "S2D", "C2S", - "L2S", "D2S", "S2S", "C2L", "L2L", "D2L", "S2L", "C2X", "L2X", "D2X", "S2X", "C2P", - "L2P", "D2P", "S2P", "C2W", "L2W", "D2W", "S2W", "C2Y", "L2Y", "D2Y", "S2Y", "C2M", - "L2M", "D2M", "S2M", "L2N", "D2N", "S2N", + "C2", "L2", "D2", "S2", "P2", "C2C", "L2C", "D2C", "S2C", "C2D", "L2D", "D2D", "S2D", + "C2S", "L2S", "D2S", "S2S", "C2L", "L2L", "D2L", "S2L", "C2X", "L2X", "D2X", "S2X", + "C2P", "L2P", "D2P", "S2P", "C2W", "L2W", "D2W", "S2W", "C2Y", "L2Y", "D2Y", "S2Y", + "C2M", "L2M", "D2M", "S2M", "L2N", "D2N", "S2N", ] } pub(crate) fn gpsl5_codes() -> [&'static str; 16] { diff --git a/rinex/src/epoch.rs b/rinex/src/epoch.rs index 8086354c1..b64dae1d5 100644 --- a/rinex/src/epoch.rs +++ b/rinex/src/epoch.rs @@ -1,13 +1,17 @@ //! Epoch parsing helpers use crate::types::Type; -use hifitime::{Duration, Epoch, TimeScale, Unit}; +use hifitime::{ + Epoch, EpochError as HifitimeEpochError, ParsingError as HifitimeParsingError, TimeScale, +}; use std::str::FromStr; use thiserror::Error; #[derive(Error, Debug)] pub enum ParsingError { - #[error("failed to parse utc timestamp")] - EpochError(#[from] hifitime::Errors), + #[error("failed to parse epoch")] + HifitimeParsingError(#[from] HifitimeParsingError), + #[error("epoch error")] + HifitimeEpochError(#[from] HifitimeEpochError), #[error("expecting \"yyyy mm dd hh mm ss.ssss\" format")] FormatError, #[error("failed to parse seconds + nanos")] @@ -39,13 +43,7 @@ pub(crate) fn now() -> Epoch { * Formats given epoch to string, matching standard specifications */ pub(crate) fn format(epoch: Epoch, t: Type, revision: u8) -> String { - // Hifitime V3 does not have a gregorian decomposition method - let (y, m, d, hh, mm, ss, nanos) = match epoch.time_scale { - TimeScale::GPST => (epoch + Duration::from_seconds(37.0)).to_gregorian_utc(), - TimeScale::GST => (epoch + Duration::from_seconds(19.0)).to_gregorian_utc(), - TimeScale::BDT => (epoch + Duration::from_seconds(19.0)).to_gregorian_utc(), - _ => epoch.to_gregorian_utc(), - }; + let (y, m, d, hh, mm, ss, nanos) = epoch_decompose(epoch); match t { Type::ObservationData => { @@ -144,9 +142,10 @@ pub(crate) fn parse_in_timescale(content: &str, ts: TimeScale) -> Result() .map_err(|_| ParsingError::YearField(item.to_string()))?; - /* old RINEX problem: YY is sometimes encoded on two digits */ + /* old RINEX problem: YY sometimes encoded on two digits */ if y < 100 { if y < 80 { + // RINEX did not exist prior 1989 y += 2000; } else { y += 1900; @@ -208,26 +207,30 @@ pub(crate) fn parse_in_timescale(content: &str, ts: TimeScale) -> Result { - // in case provided content is totally invalid, - // Epoch::from_gregorian may panic + // Catch possible Hifitime panic on bad string content if y == 0 { return Err(ParsingError::FormatError); } - let epoch = Epoch::from_gregorian_utc(y, m, d, hh, mm, ss, ns as u32); Ok(epoch) }, - _ => { - // in case provided content is totally invalid, - // Epoch::from_string may panic + TimeScale::TAI => { + // Catch possible Hifitime panic on bad string content + if y == 0 { + return Err(ParsingError::FormatError); + } + let epoch = Epoch::from_gregorian_tai(y, m, d, hh, mm, ss, ns as u32); + Ok(epoch) + }, + ts => { + // Catch possible Hifitime panic on bad string content if y == 0 { return Err(ParsingError::FormatError); } - let epoch = Epoch::from_str(&format!( - "{:04}-{:02}-{:02}T{:02}:{:02}:{:02}.{:09} {}", + let epoch = Epoch::from_gregorian_str(&format!( + "{:04}-{:02}-{:02}T{:02}:{:02}:{:02}.{:06} {}", y, m, d, hh, mm, ss, ns, ts ))?; Ok(epoch) @@ -241,16 +244,36 @@ pub(crate) fn parse_utc(s: &str) -> Result { /* * Until Hifitime provides a decomposition method in timescale other than UTC - * we have this tweak to decompose %Y %M %D %HH %MM %SS %NS + * we have this tweak to decompose %Y %M %D %HH %MM %SS and without nanoseconds */ pub(crate) fn epoch_decompose(e: Epoch) -> (i32, u8, u8, u8, u8, u8, u32) { - let ts = e.time_scale; - let offset = if ts.is_gnss() { - 37 * Unit::Second - } else { - Duration::ZERO - }; - (e + offset).to_gregorian_utc() + let isofmt = e.to_gregorian_str(e.time_scale); + let mut datetime = isofmt.split('T'); + let date = datetime.next().unwrap(); + let mut date = date.split('-'); + + let time = datetime.next().unwrap(); + let mut time_scale = time.split(' '); + let time = time_scale.next().unwrap(); + let mut time = time.split(':'); + + let years = date.next().unwrap().parse::().unwrap(); + let months = date.next().unwrap().parse::().unwrap(); + let days = date.next().unwrap().parse::().unwrap(); + + let hours = time.next().unwrap().parse::().unwrap(); + let mins = time.next().unwrap().parse::().unwrap(); + let seconds = f64::from_str(time.next().unwrap()).unwrap(); + + ( + years, + months, + days, + hours, + mins, + seconds.floor() as u8, + (seconds.fract() * 1E9).round() as u32, + ) } #[cfg(test)] diff --git a/rinex/src/header.rs b/rinex/src/header.rs index 33899efce..33441d954 100644 --- a/rinex/src/header.rs +++ b/rinex/src/header.rs @@ -9,7 +9,8 @@ use crate::{ fmt_comment, fmt_rinex, ground_position::GroundPosition, hardware::{Antenna, Rcvr, SvAntenna}, - ionex, leap, + ionex, + leap::{Error as LeapParsingError, Leap}, linspace::{Error as LinspaceError, Linspace}, marker::{GeodeticMarker, MarkerType}, merge::{ @@ -96,7 +97,7 @@ pub struct Header { /// Optional COSPAR number (launch information) pub cospar: Option, /// optionnal leap seconds infos - pub leap: Option, + pub leap: Option, // /// Optionnal system time correction // pub time_corrections: Option, /// Station approximate coordinates @@ -165,7 +166,7 @@ pub enum ParsingError { #[error("failed to parse \"{0}\" coordinates from \"{1}\"")] CoordinatesParsing(String, String), #[error("failed to parse leap from \"{0}\"")] - LeapParsingError(#[from] leap::Error), + LeapParsingError(#[from] LeapParsingError), #[error("failed to parse antenna / receiver infos")] AntennaRcvrError(#[from] std::io::Error), #[error("failed to parse ANTEX fields")] @@ -252,7 +253,7 @@ impl Header { let mut rcvr: Option = None; let mut rcvr_antenna: Option = None; let mut sv_antenna: Option = None; - let mut leap: Option = None; + let mut leap: Option = None; let mut sampling_interval: Option = None; let mut ground_position: Option = None; let mut dcb_compensations: Vec = Vec::new(); @@ -630,7 +631,7 @@ impl Header { } } else if marker.contains("LEAP SECOND") { let leap_str = content.split_at(40).0.trim(); - if let Ok(lleap) = leap::Leap::from_str(leap_str) { + if let Ok(lleap) = Leap::from_str(leap_str) { leap = Some(lleap) } } else if marker.contains("DOI") { @@ -851,8 +852,7 @@ impl Header { if interval > 0.0 { // INTERVAL = '0' may exist, in case // of Varying TEC map intervals - sampling_interval = - Some(Duration::from_f64(interval, hifitime::Unit::Second)); + sampling_interval = Some(Duration::from_seconds(interval)); } } } else if marker.contains("COSPAR NUMBER") { diff --git a/rinex/src/leap.rs b/rinex/src/leap.rs index 2a90e8427..c31ba8729 100644 --- a/rinex/src/leap.rs +++ b/rinex/src/leap.rs @@ -1,5 +1,5 @@ //! Describes `leap` second information, contained in `header` -use hifitime::TimeScale; +use hifitime::{ParsingError, TimeScale}; use thiserror::Error; /// `Leap` to describe leap seconds. @@ -27,7 +27,7 @@ pub enum Error { #[error("failed to parse leap integer number")] ParseIntError(#[from] std::num::ParseIntError), #[error("failed to parse leap timescale")] - TimeScaleError(#[from] hifitime::Errors), + TimeScaleError(#[from] ParsingError), } impl Leap { diff --git a/rinex/src/lib.rs b/rinex/src/lib.rs index 4a9070f85..ae08f1b9f 100644 --- a/rinex/src/lib.rs +++ b/rinex/src/lib.rs @@ -2286,7 +2286,7 @@ impl Rinex { }, _ => { /* determine toe */ - eph.toe(ts) + eph.toe_gpst(ts) }, }; //TODO : this fails at this point diff --git a/rinex/src/merge.rs b/rinex/src/merge.rs index 85e9b5288..b9adac667 100644 --- a/rinex/src/merge.rs +++ b/rinex/src/merge.rs @@ -1,5 +1,6 @@ //! RINEX File merging (combination) use crate::prelude::Epoch; +use hifitime::EpochError; use std::cmp::{Eq, PartialEq}; use std::collections::HashMap; use std::hash::Hash; @@ -21,7 +22,7 @@ pub enum Error { #[error("cannot merge ionex where base radius differs")] IonexBaseRadiusMismatch, #[error("failed to retrieve system time for merge ops date")] - HifitimeError(#[from] hifitime::Errors), + HifitimeError(#[from] EpochError), } /* diff --git a/rinex/src/navigation/ephemeris.rs b/rinex/src/navigation/ephemeris.rs index e9eea6898..b35cd5207 100644 --- a/rinex/src/navigation/ephemeris.rs +++ b/rinex/src/navigation/ephemeris.rs @@ -156,22 +156,25 @@ impl Ephemeris { }, } } - /* - * Retrieves and express TOE as an hifitime Epoch - */ - pub(crate) fn toe(&self, ts: TimeScale) -> Option { - /* toe week counter */ - let mut week = self.get_week()?; - if ts == TimeScale::GST { - /* Galileo vehicles stream week counter referenced to GPST.. */ - week -= 1024; + /// Retrieve and express Time of Ephemeris as a GPST Epoch + pub fn toe_gpst(&self, sv_ts: TimeScale) -> Option { + let mut week = self.get_week()?; // week counter + match sv_ts { + TimeScale::GST => { + /* Galileo vehicles stream week counter referenced to GPST.. */ + week -= 1024; + }, + _ => {}, } // "toe" field is seconds within current week to obtain toe let secs_dur = self.get_orbit_f64("toe")?; let week_dur = (week * 7) as f64 * Unit::Day; - Some(Epoch::from_duration(week_dur + secs_dur * Unit::Second, ts)) + Some( + Epoch::from_duration(week_dur + secs_dur * Unit::Second, sv_ts) + .to_time_scale(TimeScale::GPST), + ) } /* * Parses ephemeris from given line iterator @@ -337,40 +340,24 @@ impl Ephemeris { s.set_orbit_f64("omegaDot", perturbations.omega_dot); s } - /* - * Kepler equation solver at desired instant "t" for given "sv" - * based off Self. Self must be correctly selected in navigation - * record. - * "t" does not have to expressed in correct timescale prior this calculation - * See [Bibliography::AsceAppendix3] and [Bibliography::JLe19] - */ - pub(crate) fn kepler2ecef(&self, sv: SV, t: Epoch) -> Option<(f64, f64, f64)> { - let mut t = t; - - /* - * if "t" is not expressed in the correct constellation, - * take that into account - */ - t.time_scale = sv.timescale()?; - - match sv.constellation { - Constellation::GPS | Constellation::QZSS => { - t -= Duration::from_seconds(18.0); // GPST(t=0) number of leap seconds @ the time - }, - Constellation::Galileo => { - t -= Duration::from_seconds(31.0); // GST(t=0) number of leap seconds @ the time - }, - Constellation::BeiDou => { - t -= Duration::from_seconds(32.0); // BDT(t=0) number of leap seconds @ the time - }, - _ => {}, // either not needed, or most probably not truly supported - } - - let toe = self.toe(t.time_scale)?; + /// Resolves Kepler Equations from broadcasted parameters + /// and obtains SV position at desired `t`. + /// Position expressed in [km] ECEF. + /// [Bibliography::AsceAppendix3] and [Bibliography::JLe19] + pub fn kepler2ecef(&self, sv: SV, t: Epoch) -> Option<(f64, f64, f64)> { + // we only support calculations in GPST at the moment + let sv_ts = sv.timescale()?; + let toe = self.toe_gpst(sv_ts)?; + let t = t.to_time_scale(TimeScale::GPST); let kepler = self.kepler()?; let perturbations = self.perturbations()?; - let t_k = (t - toe).to_seconds(); + let mut t_k = (t - toe).to_seconds(); + if t_k > 302400.0 { + t_k -= 604800.0; + } else if t_k < -302400.0 { + t_k += 604800.0; + } let n0 = (Kepler::EARTH_GM_CONSTANT / kepler.a.powf(3.0)).sqrt(); let n = n0 + perturbations.dn; @@ -484,16 +471,13 @@ impl Ephemeris { reference.to_ecef_wgs84(), )) } - /* - * Returns max time difference between an Epoch and - * related Time of Issue of Ephemeris, for each constellation. - */ - pub(crate) fn max_dtoe(c: Constellation) -> Option { + /// Returns Ephemeris validity duration for this Constellation + pub fn max_dtoe(c: Constellation) -> Option { match c { Constellation::GPS | Constellation::QZSS => Some(Duration::from_seconds(7200.0)), Constellation::Galileo => Some(Duration::from_seconds(10800.0)), Constellation::BeiDou => Some(Duration::from_seconds(21600.0)), - Constellation::IRNSS => Some(Duration::from_seconds(86400.0)), + Constellation::IRNSS => Some(Duration::from_seconds(7200.0)), Constellation::Glonass => Some(Duration::from_seconds(1800.0)), c => { if c.is_sbas() { @@ -892,7 +876,7 @@ mod test { let descriptors: Vec<&str> = vec![ r#" { - "epoch": "2020-12-31T23:59:44.000000000 UTC", + "epoch": "2020-12-31T23:59:44.000000000 GPST", "sv": { "prn": 7, "constellation": "GPS" @@ -925,7 +909,7 @@ mod test { }"#, r#" { - "epoch": "2021-01-02T00:00:00.000000000 UTC", + "epoch": "2021-01-02T00:00:00.000000000 GPST", "sv": { "prn": 18, "constellation": "GPS" @@ -958,7 +942,7 @@ mod test { }"#, r#" { - "epoch": "2021-01-02T00:00:00.000000000 UTC", + "epoch": "2021-01-02T00:00:00.000000000 GPST", "sv": { "prn": 30, "constellation": "GPS" @@ -991,7 +975,7 @@ mod test { }"#, r#" { - "epoch": "2021-12-31T22:00:00.000000000 UTC", + "epoch": "2021-12-31T22:00:00.000000000 GPST", "sv": { "prn": 8, "constellation": "GPS" @@ -1024,7 +1008,7 @@ mod test { }"#, r#" { - "epoch": "2022-01-01T00:00:00.000000000 UTC", + "epoch": "2022-01-01T00:00:00.000000000 GPST", "sv": { "prn": 32, "constellation": "GPS" @@ -1057,7 +1041,7 @@ mod test { }"#, r#" { - "epoch": "2021-12-30T20:00:00.000000000 UTC", + "epoch": "2021-12-30T20:00:00.000000000 GPST", "sv": { "prn": 11, "constellation": "GPS" @@ -1122,9 +1106,24 @@ mod test { let x_err = (ecef.0 * 1000.0 - helper.ecef.0).abs(); let y_err = (ecef.1 * 1000.0 - helper.ecef.1).abs(); let z_err = (ecef.2 * 1000.0 - helper.ecef.2).abs(); - assert!(x_err < 1E-6, "kepler2ecef: x_err too large: {}", x_err); - assert!(y_err < 1E-6, "kepler2ecef: y_err too large: {}", y_err); - assert!(z_err < 1E-6, "kepler2ecef: z_err too large: {}", z_err); + assert!( + x_err < 1E-6, + "kepler2ecef@{}: x_err too large: {}", + helper.epoch, + x_err + ); + assert!( + y_err < 1E-6, + "kepler2ecef@{}: y_err too large: {}", + helper.epoch, + y_err + ); + assert!( + z_err < 1E-6, + "kepler2ecef@{}: z_err too large: {}", + helper.epoch, + z_err + ); let el_az = ephemeris.sv_elev_azim(helper.sv, helper.epoch, helper.ref_pos); assert!( diff --git a/rinex/src/tests/decimation.rs b/rinex/src/tests/decimation.rs index bccf9d95a..7121f9a53 100644 --- a/rinex/src/tests/decimation.rs +++ b/rinex/src/tests/decimation.rs @@ -83,10 +83,10 @@ mod decimation { rinex.decimate_by_interval_mut(Duration::from_seconds(60.0)); let count = rinex.epoch().count(); - assert_eq!(count, 1016, "decimate(1'): error",); + assert_eq!(count, 1013, "decimate(1'): error",); rinex.decimate_by_interval_mut(Duration::from_seconds(61.0)); let count = rinex.epoch().count(); - assert_eq!(count, 1016, "decimate(1'+1s): error",); + assert_eq!(count, 1013, "decimate(1'+1s): error",); } } diff --git a/rinex/src/tests/nav.rs b/rinex/src/tests/nav.rs index cc6185970..0c4ca4eb4 100644 --- a/rinex/src/tests/nav.rs +++ b/rinex/src/tests/nav.rs @@ -7,6 +7,7 @@ mod test { use crate::tests::toolkit::nav::check_nequick_g_models; use gnss_rs::prelude::SV; use gnss_rs::sv; + use hifitime::Unit; use itertools::*; use std::path::Path; use std::path::PathBuf; @@ -1442,7 +1443,32 @@ mod test { } #[test] #[cfg(feature = "nav")] - fn sv_toe_ephemeris() { + fn toe_ephemeris_glo() { + let path = Path::new(env!("CARGO_MANIFEST_DIR")) + .join("..") + .join("test_resources") + .join("NAV") + .join("V2") + .join("dlf10010.21g"); + let rinex = Rinex::from_file(path.to_string_lossy().as_ref()); + assert!(rinex.is_ok()); + let rinex = rinex.unwrap(); + for (toc, (_, sv, ephemeris)) in rinex.ephemeris() { + match sv.prn { + 3 => {}, + 17 => {}, + 1 => {}, + 18 => {}, + 19 => {}, + 8 => {}, + 16 => {}, + _ => panic!("found unexpected SV"), + } + } + } + #[test] + #[cfg(feature = "nav")] + fn toe_ephemeris_gal_bds() { let path = Path::new(env!("CARGO_MANIFEST_DIR")) .join("..") .join("test_resources") @@ -1462,34 +1488,28 @@ mod test { assert!(ts.is_some(), "timescale should be determined"); let ts = ts.unwrap(); - if let Some(toe) = ephemeris.toe(ts) { - let mut expected_sv = SV::default(); - let mut expected_toe = Epoch::default(); + if let Some(toe) = ephemeris.toe_gpst(ts) { if *toc == e0 { - expected_toe = Epoch::from_str("2021-01-01T00:00:33 BDT").unwrap(); - expected_sv = sv!("C05"); + let expected = toe_helper(0.782E3, 0.432E6, TimeScale::BDT); + assert_eq!(toe, expected.to_time_scale(TimeScale::GPST),); } else if *toc == e1 { - expected_toe = Epoch::from_str("2021-01-01T05:00:33 BDT").unwrap(); - expected_sv = sv!("C21"); + let expected = toe_helper(0.782E3, 0.450E6, TimeScale::BDT); + assert_eq!(toe, expected.to_time_scale(TimeScale::GPST),); } else if *toc == e2 { - expected_toe = Epoch::from_str("2021-01-01T10:10:19 GST").unwrap(); - expected_sv = sv!("E01"); + let expected = toe_helper(0.2138E4, 0.4686E6, TimeScale::GST); + assert_eq!(toe, expected.to_time_scale(TimeScale::GPST),); } else if *toc == e3 { - expected_toe = Epoch::from_str("2021-01-01T15:40:19 GST").unwrap(); - expected_sv = sv!("E03"); - } else { - panic!("unhandled toc {}", toc); + let expected = toe_helper(0.2138E4, 0.4884E6, TimeScale::GST); + assert_eq!(toe, expected.to_time_scale(TimeScale::GPST),); } - assert_eq!(sv, expected_sv, "wrong sv"); - assert_eq!(toe, expected_toe, "wrong toe evaluated"); - /* - * Rinex.sv_ephemeris(@ toe) should return exact ephemeris - */ - assert_eq!( - rinex.sv_ephemeris(expected_sv, toe), - Some((expected_toe, ephemeris)), - "sv_ephemeris(sv,t) @ toe should strictly identical ephemeris" - ); + // /* + // * Rinex.sv_ephemeris(@ toe) should propose that very same ephemeris + // */ + //assert_eq!( + // rinex.sv_ephemeris(expected_sv, toe), + // Some((expected_toe, ephemeris)), + // "sv_ephemeris(sv,t) @ toe should strictly identical ephemeris" + //); } } } @@ -1552,7 +1572,7 @@ mod test { } #[test] #[cfg(feature = "nav")] - fn v2_ion_alpha_beta() { + fn v2_iono_alphabeta_and_toe() { let path = PathBuf::new() .join(env!("CARGO_MANIFEST_DIR")) .join("..") @@ -1567,7 +1587,56 @@ mod test { rinex.err() ); let rinex = rinex.unwrap(); - // Earliest record epoch is 2020-12-31 23:59:44 + // Earliest epoch record is 2020-12-31 23:59:44 + + for (toc, (_, sv, ephemeris)) in rinex.ephemeris() { + let sv_ts = sv.timescale().unwrap(); + let toe_gpst = ephemeris.toe_gpst(sv_ts).unwrap(); + match toc.to_string().as_str() { + "2021-01-01T02:00:00 GPST" => { + assert_eq!(sv.prn, 1, "found invalid vehicle"); + let toe = toe_helper(2.138000000000E3, 4.392000000000E5, TimeScale::GPST); + assert_eq!(toe_gpst, toe); + }, + "2021-12-31T23:59:44 GPST" => { + if sv.prn == 7 { + let toe = toe_helper(2.138000000000E3, 4.319840000000E5, TimeScale::GPST); + assert_eq!(toe_gpst, toe); + } else if sv.prn == 8 { + let toe = toe_helper(2.138000000000E3, 4.391840000000E5, TimeScale::GPST); + assert_eq!(toe_gpst, toe); + } else { + panic!("found invalid vehicle"); + } + }, + "2021-01-01T00:00:00 GPST" => { + assert_eq!(sv.prn, 8, "found invalid vehicle"); + let toe = toe_helper(2.138000000000E3, 4.320000000000E5, TimeScale::GPST); + assert_eq!(toe_gpst, toe); + }, + "2021-01-01T01:59:44 GPST" => { + if sv.prn == 7 { + let toe = toe_helper(2.138000000000E3, 4.391840000000E5, TimeScale::GPST); + assert_eq!(toe_gpst, toe); + } else if sv.prn == 8 { + let toe = toe_helper(2.138000000000E3, 4.391840000000E5, TimeScale::GPST); + assert_eq!(toe_gpst, toe); + } else { + panic!("found invalid vehicle"); + } + }, + "2021-01-02T00:00:00 GPST" => { + if sv.prn == 30 { + let toe = toe_helper(2.138000000000E3, 5.184000000000E5, TimeScale::GPST); + assert_eq!(toe_gpst, toe); + } else if sv.prn == 5 { + let toe = toe_helper(2.138000000000E3, 5.184000000000E5, TimeScale::GPST); + assert_eq!(toe_gpst, toe); + } + }, + _ => {}, + } + } for (t0, should_work) in [ // MIDNIGHT T0 exact match @@ -1706,4 +1775,12 @@ mod test { } } } + // Computes TOE in said timescale + fn toe_helper(week: f64, week_s: f64, ts: TimeScale) -> Epoch { + if ts == TimeScale::GST { + Epoch::from_duration((week - 1024.0) * Unit::Week + week_s * Unit::Second, ts) + } else { + Epoch::from_duration(week * Unit::Week + week_s * Unit::Second, ts) + } + } } diff --git a/sinex/Cargo.toml b/sinex/Cargo.toml index 879e367b2..582414cdf 100644 --- a/sinex/Cargo.toml +++ b/sinex/Cargo.toml @@ -20,4 +20,5 @@ chrono = "0.4" thiserror = "1" strum = { version = "0.26", features = ["derive"] } strum_macros = "0.26" -gnss-rs = { version = "2.1.3", features = ["serde"] } +# gnss-rs = { version = "2.1.3", features = ["serde"] } +gnss-rs = { git = "https://github.com/rtk-rs/gnss", branch = "main", features = ["serde"] } diff --git a/sp3/Cargo.toml b/sp3/Cargo.toml index cb7eb68ca..96fd826ef 100644 --- a/sp3/Cargo.toml +++ b/sp3/Cargo.toml @@ -24,8 +24,9 @@ rustdoc-args = ["--cfg", "docrs", "--generate-link-to-definition"] [dependencies] thiserror = "1" map_3d = "0.1.5" -hifitime = "3.9.0" -gnss-rs = { version = "2.1.3", features = ["serde"] } +hifitime = { git = "https://github.com/nyx-space/hifitime.git", branch = "master" } +# gnss-rs = { version = "2.1.3", features = ["serde"] } +gnss-rs = { git = "https://github.com/rtk-rs/gnss", branch = "main", features = ["serde"] } serde = { version = "1.0", optional = true, default-features = false, features = ["derive"] } flate2 = { version = "1.0.24", optional = true, default-features = false, features = ["zlib"] } diff --git a/sp3/src/lib.rs b/sp3/src/lib.rs index c11293523..2ec4e1616 100644 --- a/sp3/src/lib.rs +++ b/sp3/src/lib.rs @@ -4,7 +4,7 @@ extern crate gnss_rs as gnss; use gnss::prelude::{Constellation, SV}; -use hifitime::{Duration, Epoch, TimeScale}; +use hifitime::{Duration, Epoch, ParsingError as EpochParsingError, TimeScale}; use std::collections::BTreeMap; use std::str::FromStr; @@ -168,8 +168,8 @@ pub struct SP3 { pub constellation: Constellation, /// File original time system, /// either UTC or time source from which we converted to UTC. - pub time_system: TimeScale, - /// Initial week counter, in time_system + pub time_scale: TimeScale, + /// Initial week counter in [TimeScale] pub week_counter: (u32, f64), /// Initial MJD, in time_system pub mjd_start: (u32, f64), @@ -198,7 +198,7 @@ pub enum Errors { #[error("parsing error")] ParsingError(#[from] ParsingError), #[error("hifitime parsing error")] - HifitimeParsingError(#[from] hifitime::Errors), + HifitimeParsingError(#[from] EpochParsingError), #[error("constellation parsing error")] ConstellationParsingError(#[from] gnss::constellation::ParsingError), #[error("unknown or non supported revision \"{0}\"")] @@ -304,7 +304,7 @@ impl SP3 { let mut version = Version::default(); let mut data_type = DataType::default(); - let mut time_system = TimeScale::default(); + let mut time_scale = TimeScale::default(); let mut constellation = Constellation::default(); let mut pc_count = 0_u8; @@ -354,13 +354,13 @@ impl SP3 { if pc_count == 0 { constellation = Constellation::from_str(line[3..5].trim())?; - time_system = TimeScale::from_str(line[9..12].trim())?; + time_scale = TimeScale::from_str(line[9..12].trim())?; } pc_count += 1; } if new_epoch(line) { - epoch = parse_epoch(&line[3..], time_system)?; + epoch = parse_epoch(&line[3..], time_scale)?; epochs.push(epoch); } if position_entry(line) { @@ -448,7 +448,7 @@ impl SP3 { version, data_type, epoch: epochs, - time_system, + time_scale, constellation, coord_system, orbit_type, @@ -654,7 +654,7 @@ impl Merge for SP3 { // if self.agency != rhs.agency { // return Err(MergeError::DataProvider); // } - if self.time_system != rhs.time_system { + if self.time_scale != rhs.time_scale { return Err(MergeError::TimeScale); } if self.coord_system != rhs.coord_system { diff --git a/sp3/src/tests/parser_3c.rs b/sp3/src/tests/parser_3c.rs index 68db56b5c..a7f85ca8d 100644 --- a/sp3/src/tests/parser_3c.rs +++ b/sp3/src/tests/parser_3c.rs @@ -38,7 +38,7 @@ mod test { assert_eq!(sp3.nb_epochs(), 96, "bad number of epochs"); assert_eq!(sp3.coord_system, "ITRF2"); assert_eq!(sp3.orbit_type, OrbitType::BHN); - assert_eq!(sp3.time_system, TimeScale::GPST); + assert_eq!(sp3.time_scale, TimeScale::GPST); assert_eq!(sp3.constellation, Constellation::Mixed); assert_eq!(sp3.agency, "ESOC"); diff --git a/sp3/src/tests/parser_3d.rs b/sp3/src/tests/parser_3d.rs index 183a3df2f..22eee302b 100644 --- a/sp3/src/tests/parser_3d.rs +++ b/sp3/src/tests/parser_3d.rs @@ -35,7 +35,7 @@ mod test { assert_eq!(sp3.nb_epochs(), 1, "bad number of epochs"); assert_eq!(sp3.coord_system, "IGS14"); assert_eq!(sp3.orbit_type, OrbitType::FIT); - assert_eq!(sp3.time_system, TimeScale::GPST); + assert_eq!(sp3.time_scale, TimeScale::GPST); assert_eq!(sp3.constellation, Constellation::Mixed); assert_eq!(sp3.agency, "IGS"); assert_eq!(sp3.week_counter, (2077, 0.0_f64)); diff --git a/ublox-rnx/Cargo.toml b/ublox-rnx/Cargo.toml index bff3abc9f..f8e7964a8 100644 --- a/ublox-rnx/Cargo.toml +++ b/ublox-rnx/Cargo.toml @@ -21,5 +21,6 @@ serde_json = "1.0" serialport = "4.2.0" ublox = "0.4.4" clap = { version = "4.4.10", features = ["derive", "color"] } -gnss-rs = { version = "2.1.3", features = ["serde"] } +# gnss-rs = { version = "2.1.3", features = ["serde"] } +gnss-rs = { git = "https://github.com/rtk-rs/gnss", branch = "main", features = ["serde"] } rinex = { path = "../rinex", version = "=0.16.1", features = ["serde", "nav", "obs", "clock"] }