Added functions for constructing objects from SBP messages (#42)

Author: jbangelo

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"

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")
             }
           }
         }

src/ephemeris.rs

@@ -377,7 +377,7 @@ impl Ephemeris {
         Ok(doppler)
     }
 
-    pub fn get_sid(&self) -> std::result::Result<GnssSignal, InvalidGnssSignal> {
+    pub fn get_sid(&self) -> Result<GnssSignal, InvalidGnssSignal> {
         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<InvalidGpsTime> for EphemerisDecodeError {
+        fn from(other: InvalidGpsTime) -> EphemerisDecodeError {
+            EphemerisDecodeError::InvalidTime(other)
+        }
+    }
+
+    impl From<InvalidGnssSignal> 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<sbp::messages::observation::MsgEphemerisGPS> for Ephemeris {
+    type Error = EphemerisDecodeError;
+
+    fn try_from(
+        eph: sbp::messages::observation::MsgEphemerisGPS,
+    ) -> Result<Ephemeris, EphemerisDecodeError> {
+        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<sbp::messages::observation::MsgEphemerisGal> for Ephemeris {
+    type Error = EphemerisDecodeError;
+
+    fn try_from(
+        eph: sbp::messages::observation::MsgEphemerisGal,
+    ) -> Result<Ephemeris, EphemerisDecodeError> {
+        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) }

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<sbp::messages::observation::PackedObsContent> for NavigationMeasurement {
+    type Error = crate::signal::InvalidGnssSignal;
+
+    fn try_from(
+        observation: sbp::messages::observation::PackedObsContent,
+    ) -> Result<NavigationMeasurement, crate::signal::InvalidGnssSignal> {
+        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

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<u8> for Constellation {
+    type Error = InvalidConstellation;
+    fn try_from(value: u8) -> Result<Constellation, InvalidConstellation> {
+        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<u8> for Code {
+    type Error = InvalidCode;
+    fn try_from(value: u8) -> Result<Code, InvalidCode> {
+        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<sbp::messages::gnss::GnssSignal> for GnssSignal {
+    type Error = InvalidGnssSignal;
+
+    fn try_from(value: sbp::messages::gnss::GnssSignal) -> Result<GnssSignal, InvalidGnssSignal> {
+        use std::convert::TryInto;
+
+        GnssSignal::new(value.sat as u16, value.code.try_into()?)
+    }
+}
+
 #[cfg(test)]
 mod tests {
     use super::*;

src/time.rs

@@ -175,6 +175,25 @@ impl SubAssign<Duration> for GpsTime {
     }
 }
 
+#[cfg(feature = "sbp-conversions")]
+impl std::convert::TryFrom<sbp::messages::gnss::GPSTime> for GpsTime {
+    type Error = InvalidGpsTime;
+
+    fn try_from(msg: sbp::messages::gnss::GPSTime) -> Result<GpsTime, InvalidGpsTime> {
+        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<sbp::messages::gnss::GPSTimeSec> for GpsTime {
+    type Error = InvalidGpsTime;
+
+    fn try_from(msg: sbp::messages::gnss::GPSTimeSec) -> Result<GpsTime, InvalidGpsTime> {
+        GpsTime::new(msg.wn as i16, msg.tow as f64)
+    }
+}
+
 #[cfg(test)]
 mod tests {
     use super::*;