diff --git a/Cargo.toml b/Cargo.toml index 75fe52c..15b6dbd 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -8,7 +8,11 @@ readme = "README.md" repository = "https://github.com/swift-nav/swiftnav-rs" license = "LGPL-3.0" +[features] +sbp-conversions = ["sbp"] + [dependencies] +sbp = { git = "https://github.com/swift-nav/libsbp.git", rev="51660bd02f18216aef7abcdebf27fec72a93c161", optional = true } [build-dependencies] bindgen = "0.57" diff --git a/Jenkinsfile b/Jenkinsfile index dc5f2b7..0e49e96 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -24,7 +24,7 @@ pipeline { steps { gitPrep() script { - sh("cargo check") + sh("cargo check --all-targets --all-features") } } } @@ -50,7 +50,7 @@ pipeline { agent { dockerfile { reuseNode true } } steps { script { - sh("cargo clippy") + sh("cargo clippy --all-targets --all-features") } } } diff --git a/src/ephemeris.rs b/src/ephemeris.rs index 79c3d00..d1bc411 100644 --- a/src/ephemeris.rs +++ b/src/ephemeris.rs @@ -377,7 +377,7 @@ impl Ephemeris { Ok(doppler) } - pub fn get_sid(&self) -> std::result::Result { + pub fn get_sid(&self) -> Result { GnssSignal::from_gnss_signal_t(self.0.sid) } @@ -405,6 +405,137 @@ impl Ephemeris { } } +#[cfg(feature = "sbp-conversions")] +mod sbp_error { + use crate::{signal::InvalidGnssSignal, time::InvalidGpsTime}; + use std::error::Error; + use std::fmt; + + #[derive(Debug, Copy, Clone, PartialOrd, PartialEq)] + pub enum EphemerisDecodeError { + InvalidTime(InvalidGpsTime), + InvalidSignal(InvalidGnssSignal), + } + + impl fmt::Display for EphemerisDecodeError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + EphemerisDecodeError::InvalidTime(time_err) => time_err.fmt(f), + EphemerisDecodeError::InvalidSignal(sig_err) => sig_err.fmt(f), + } + } + } + + impl Error for EphemerisDecodeError {} + + impl From for EphemerisDecodeError { + fn from(other: InvalidGpsTime) -> EphemerisDecodeError { + EphemerisDecodeError::InvalidTime(other) + } + } + + impl From for EphemerisDecodeError { + fn from(other: InvalidGnssSignal) -> EphemerisDecodeError { + EphemerisDecodeError::InvalidSignal(other) + } + } +} + +#[cfg(feature = "sbp-conversions")] +pub use sbp_error::EphemerisDecodeError; + +#[cfg(feature = "sbp-conversions")] +impl std::convert::TryFrom for Ephemeris { + type Error = EphemerisDecodeError; + + fn try_from( + eph: sbp::messages::observation::MsgEphemerisGPS, + ) -> Result { + use std::convert::TryInto; + + Ok(Ephemeris::new( + eph.common.sid.try_into()?, + eph.common.toe.try_into()?, + eph.common.ura, + eph.common.fit_interval, + eph.common.valid, + eph.common.health_bits, + 0, + EphemerisTerms::new_kepler( + Constellation::Gps, + [eph.tgd, 0.], + eph.c_rc as f64, + eph.c_rs as f64, + eph.c_uc as f64, + eph.c_us as f64, + eph.c_ic as f64, + eph.c_is as f64, + eph.dn, + eph.m0, + eph.ecc, + eph.sqrta, + eph.omega0, + eph.omegadot, + eph.w, + eph.inc, + eph.inc_dot, + eph.af0 as f64, + eph.af1 as f64, + eph.af2 as f64, + eph.toc.try_into()?, + eph.iodc, + eph.iode as u16, + ), + )) + } +} + +#[cfg(feature = "sbp-conversions")] +impl std::convert::TryFrom for Ephemeris { + type Error = EphemerisDecodeError; + + fn try_from( + eph: sbp::messages::observation::MsgEphemerisGal, + ) -> Result { + use std::convert::TryInto; + + Ok(Ephemeris::new( + eph.common.sid.try_into()?, + eph.common.toe.try_into()?, + eph.common.ura, + eph.common.fit_interval, + eph.common.valid, + eph.common.health_bits, + eph.source, + EphemerisTerms::new_kepler( + Constellation::Gal, + [eph.bgd_e1e5a, eph.bgd_e1e5b], + eph.c_rc as f64, + eph.c_rs as f64, + eph.c_uc as f64, + eph.c_us as f64, + eph.c_ic as f64, + eph.c_is as f64, + eph.dn, + eph.m0, + eph.ecc, + eph.sqrta, + eph.omega0, + eph.omegadot, + eph.w, + eph.inc, + eph.inc_dot, + eph.af0 as f64, + eph.af1 as f64, + eph.af2 as f64, + eph.toc.try_into()?, + eph.iodc, + eph.iode as u16, + ), + )) + } +} + impl PartialEq for Ephemeris { fn eq(&self, other: &Self) -> bool { unsafe { c_bindings::ephemeris_equal(&self.0, &other.0) } diff --git a/src/navmeas.rs b/src/navmeas.rs index 1c600d1..0190e74 100644 --- a/src/navmeas.rs +++ b/src/navmeas.rs @@ -15,6 +15,8 @@ use std::time::Duration; const NAV_MEAS_FLAG_CODE_VALID: u16 = 1 << 0; const NAV_MEAS_FLAG_MEAS_DOPPLER_VALID: u16 = 1 << 2; const NAV_MEAS_FLAG_CN0_VALID: u16 = 1 << 5; +#[allow(dead_code)] +const NAV_MEAS_FLAG_RAIM_EXCLUSION: u16 = 1 << 6; /// Represents a single raw GNSS measurement #[derive(Debug, Clone, PartialOrd, PartialEq)] @@ -141,6 +143,38 @@ impl Default for NavigationMeasurement { } } +#[cfg(feature = "sbp-conversions")] +impl std::convert::TryFrom for NavigationMeasurement { + type Error = crate::signal::InvalidGnssSignal; + + fn try_from( + observation: sbp::messages::observation::PackedObsContent, + ) -> Result { + use std::convert::TryInto; + + let mut measurement = NavigationMeasurement::new(); + + measurement.set_lock_time(decode_lock_time(observation.lock)); + measurement.set_sid(observation.sid.try_into()?); + // A CN0 of 0 is considered invalid + if observation.cn0 != 0 { + measurement.set_cn0(observation.cn0 as f64 / 4.); + } + if observation.flags & 0x01 != 0 { + measurement.set_pseudorange(observation.P as f64 / 5e1); + } + if observation.flags & 0x08 != 0 { + measurement + .set_measured_doppler(observation.D.i as f64 + (observation.D.f as f64) / 256.); + } + if observation.flags & 0x80 != 0 { + measurement.0.flags |= NAV_MEAS_FLAG_RAIM_EXCLUSION; + } + + Ok(measurement) + } +} + /// Encodes a [`Duration`] as an SBP lock time /// /// Note: It is encoded according to DF402 from the RTCM 10403.2 Amendment 2 diff --git a/src/signal.rs b/src/signal.rs index 92c54e1..0405807 100644 --- a/src/signal.rs +++ b/src/signal.rs @@ -29,7 +29,7 @@ pub enum Constellation { /// Invalid constellation integer value #[derive(Debug, Copy, Clone, Ord, PartialOrd, Eq, PartialEq, Hash)] -struct InvalidConstellation(c_bindings::constellation_t); +pub struct InvalidConstellation(c_bindings::constellation_t); impl fmt::Display for InvalidConstellation { fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { @@ -82,6 +82,13 @@ impl Constellation { } } +impl std::convert::TryFrom for Constellation { + type Error = InvalidConstellation; + fn try_from(value: u8) -> Result { + Self::from_constellation_t(value as c_bindings::constellation_t) + } +} + /// Code identifiers #[derive(Debug, Copy, Clone, Ord, PartialOrd, Eq, PartialEq, Hash)] pub enum Code { @@ -395,6 +402,13 @@ impl Code { } } +impl std::convert::TryFrom for Code { + type Error = InvalidCode; + fn try_from(value: u8) -> Result { + Self::from_code_t(value as c_bindings::code_t) + } +} + /// GNSS Signal identifier #[derive(Debug, Copy, Clone, Ord, PartialOrd, Eq, PartialEq, Hash)] pub struct GnssSignal(c_bindings::gnss_signal_t); @@ -469,6 +483,17 @@ impl GnssSignal { } } +#[cfg(feature = "sbp-conversions")] +impl std::convert::TryFrom for GnssSignal { + type Error = InvalidGnssSignal; + + fn try_from(value: sbp::messages::gnss::GnssSignal) -> Result { + use std::convert::TryInto; + + GnssSignal::new(value.sat as u16, value.code.try_into()?) + } +} + #[cfg(test)] mod tests { use super::*; diff --git a/src/time.rs b/src/time.rs index 208e8d5..052b39c 100644 --- a/src/time.rs +++ b/src/time.rs @@ -175,6 +175,25 @@ impl SubAssign for GpsTime { } } +#[cfg(feature = "sbp-conversions")] +impl std::convert::TryFrom for GpsTime { + type Error = InvalidGpsTime; + + fn try_from(msg: sbp::messages::gnss::GPSTime) -> Result { + let tow = (msg.tow as f64) * 1e-3 + (msg.ns_residual as f64) * 1e-9; + GpsTime::new(msg.wn as i16, tow) + } +} + +#[cfg(feature = "sbp-conversions")] +impl std::convert::TryFrom for GpsTime { + type Error = InvalidGpsTime; + + fn try_from(msg: sbp::messages::gnss::GPSTimeSec) -> Result { + GpsTime::new(msg.wn as i16, msg.tow as f64) + } +} + #[cfg(test)] mod tests { use super::*;