gnss: Add support for ublox GNSS

src/ubx.rs

@@ -0,0 +1,106 @@
+//! u-blox helpers
+#![allow(unused)]
+
+use alloc::vec::Vec;
+
+pub fn pretty_print_mon_ver(packet: &ublox::mon_ver::MonVerRef) -> alloc::string::String {
+    alloc::format!(
+        "UBX-MON-VER: SW version: {} HW version: {}; Extensions: {:?}",
+        packet.software_version(),
+        packet.hardware_version(),
+        packet.extension().collect::<Vec<&str>>()
+    )
+}
+
+pub fn pretty_print_mon_comms(packet: &ublox::mon_comms::MonCommsRef) -> alloc::string::String {
+    alloc::format!(
+        "UBX-MON-COMMS: {} ports ({:?}), {} tx err, info {:?}",
+        packet.n_ports(),
+        packet.prot_ids(),
+        packet.tx_errors(),
+        packet
+            .ports()
+            .map(|v| alloc::format!("{v:?}"))
+            .collect::<Vec<_>>()
+    )
+}
+
+pub fn pretty_print_mon_rf(packet: &ublox::mon_rf::MonRfRef) -> alloc::string::String {
+    alloc::format!(
+        "UBX-MON-RF: {:?}",
+        packet
+            .blocks()
+            .map(|v| alloc::format!("{v:?}"))
+            .collect::<Vec<_>>()
+    )
+}
+
+pub fn pretty_print_nav_sat(packet: &ublox::nav_sat::NavSatRef) -> alloc::string::String {
+    // sort by quality flags
+    let mut sat_infos = packet.svs().collect::<Vec<_>>();
+    sat_infos.sort_unstable_by_key(|v| v.flags_raw() & 0x7);
+    sat_infos.reverse();
+
+    let sat_info_strings = sat_infos
+        .iter()
+        .map(|v| {
+            alloc::format!(
+                "- {}/{:3}: E {:3.0}, A {:3.0}, CNO {:2.1} dBHz, {:?}, {:?}",
+                v.gnss_id(),
+                v.sv_id(),
+                v.elev(),
+                v.azim(),
+                v.cno(),
+                v.flags().quality_ind(),
+                v.flags().health(),
+            )
+        })
+        .collect::<Vec<_>>();
+
+    alloc::format!(
+        "UBX-NAV-SAT: num {:?}\n{}",
+        packet.num_svs(),
+        sat_info_strings.join("\n")
+    )
+}
+
+impl<'a> TryFrom<&ublox::nav_pvt::proto27::NavPvtRef<'a>> for crate::applogic::GnssFix {
+    type Error = alloc::string::String;
+
+    fn try_from(value: &ublox::nav_pvt::proto27::NavPvtRef<'a>) -> Result<Self, Self::Error> {
+        let has_time = value.fix_type() == ublox::GnssFixType::Fix3D
+            || value.fix_type() == ublox::GnssFixType::GPSPlusDeadReckoning
+            || value.fix_type() == ublox::GnssFixType::TimeOnlyFix;
+        let has_posvel = value.fix_type() == ublox::GnssFixType::Fix3D
+            || value.fix_type() == ublox::GnssFixType::GPSPlusDeadReckoning;
+
+        if !has_posvel || !has_time {
+            return Err(alloc::format!(
+                "NAV-PVT without valid fix: {:?}",
+                value.fix_type()
+            ));
+        }
+
+        let pos: ublox::PositionLLA = value.into();
+        let vel: ublox::Velocity = value.into();
+
+        let time: chrono::DateTime<chrono::Utc> = value
+            .try_into()
+            .map_err(|err| alloc::format!("Failed to convert time: {err}"))?;
+
+        let latitude_deg = pos.lat;
+        let longitude_deg = pos.lon;
+        #[allow(clippy::cast_precision_loss)]
+        let heading_deg = vel.heading as f32;
+        #[allow(clippy::cast_precision_loss)]
+        let speed_mps = vel.speed as f32;
+
+        Ok(Self {
+            time: Some(time.naive_utc()),
+            latitude_deg,
+            longitude_deg,
+            heading_deg: Some(heading_deg),
+            speed_mps: Some(speed_mps),
+        })
+    }
+}