gnss: Add support for ublox GNSS

Cargo.lock

@@ -1178,6 +1178,7 @@ dependencies = [
  "profont",
  "prost",
  "prost-build",
+ "ublox",
 ]
 
 [[package]]
@@ -2490,6 +2491,29 @@ version = "1.20.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "40ce102ab67701b8526c123c1bab5cbe42d7040ccfd0f64af1a385808d2f43de"
 
+[[package]]
+name = "ublox"
+version = "0.10.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5bcc96bd937066775fb87034e1dbf6d7165a8975c74ddec1105e31b4a050538b"
+dependencies = [
+ "bitflags",
+ "chrono",
+ "num-traits",
+ "ublox_derive",
+]
+
+[[package]]
+name = "ublox_derive"
+version = "0.6.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f5eda7d1c1151a40ae85c7c065a19959909573535034e18e1015b738993cd2c0"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn 1.0.109",
+]
+
 [[package]]
 name = "ufmt-write"
 version = "0.1.0"

Cargo.toml

@@ -15,7 +15,7 @@ path = "./src/test.rs"
 required-features = ["std", "cam"]
 
 [features]
-default = ["esp", "spat", "gnss", "screen"]
+default = ["esp", "spat", "gnss_ubx", "screen"]
 
 # build for ESP (needed to build test for `host-tuple`)
 esp = [
@@ -33,8 +33,10 @@ esp = [
 ]
 std = ["c-its-parser/std"]
 
+# enable u-blox GNSS module
+gnss_ubx = ["_gnss", "_uart", "dep:ublox"]
 # enable L67K GNSS module
-gnss = ["_gnss", "dep:embassy_gps"]
+gnss_l67k = ["_gnss", "dep:embassy_gps"]
 # echo CAM data on serial
 cam = ["c-its-parser/cam"]
 # send CAM autonomously
@@ -107,6 +109,7 @@ mipidsi = { version = "0.10.0", optional = true }
 num-traits = { version = "0.2.19", default-features = false, features = ["libm"] }
 profont = { version = "0.7.0", optional = true }
 prost = { version = "0.14", optional = true, default-features = false, features = ["derive"] }
+ublox = { version = "0.10", optional = true, default-features = false, features = ["alloc", "ubx_proto27"] }
 
 [dev-dependencies]
 assert_float_eq = "1.2.0"

Readme.md

@@ -5,15 +5,16 @@ Displays different C-ITS receive-only use cases for cheap.
 ## Hardware Requirements
 The code is currently tailored to:
 - Seeed XIAO ESP32-C5 (8MB PSRAM)
-- Seeed XIAO L67K (connected on GPIO12 (RX), GPIO11 (TX), GPIO1 (wakeup) and GPIO25 (reset))  
+- U-Blox M10Q GNSS (connected on GPIO12 (RX), GPIO11 (TX), and GPIO25 (reset))  
-only available with `gnss` feature (which is included in `spat` feature)
+or Seeed XIAO L67K (connected on GPIO12 (RX), GPIO11 (TX), GPIO1 (wakeup) and GPIO25 (reset))  
+only available with `gnss_ubx` or `gnss_l67k` feature (which is included in `spat` feature)
 - Some ST7789 172*320px screen (connected on GPIO8 (SCL/ SCK), GPIO10 (SDA/ MOSI), GPIO7 (reset), GPIO23 (DC), GPIO24 (CS), TODO GPIO0 (BL))  
 only available with `screen` feature (which is included in `spat` feature)
 
 ## Features
 
 ### GNSS
-Enable L67K GNSS module and in main application logic/ state.
+Enable u-blox or L67K GNSS module and in main application logic/ state.
 
 ### SPAT
 Takes the current position and heading from the GNSS receiver and displays the signal phases of the upcoming intersection.
@@ -89,9 +90,9 @@ Additional features are available through feature flags:
 - `cam_tx`: Enable CAM generation and transmission (needs time and position source)
 
 Officially supported feature combinations are:
-- `esp,spat,gnss,screen` (default features, "standalone")
+- `esp,spat,gnss_ubx,screen` (default features, "standalone")
-- `esp,spat,gnss,screen,cam_tx` ("standalone with CAM")
+- `esp,spat,gnss_ubx,screen,cam_tx` ("standalone with CAM")
-- `esp,spat,gnss,screen,cam_tx,cam,denm`
+- `esp,spat,gnss_ubx,screen,cam_tx,cam,denm`
 - `esp,uart,cam,denm` ("smartphone companion")
 
 Connect your ESP32-C5 and run:

feat-permut.sh

@@ -9,12 +9,12 @@ extended=${2:-false}
 
 features_base=(
   uart,cam,denm
-  spat,gnss,screen
+  spat,gnss_ubx,screen
-  spat,gnss,screen,cam_tx
+  spat,gnss_ubx,screen,cam_tx
 )
 features_more=(
-  spat,gnss,screen,cam,denm
+  spat,gnss_ubx,screen,cam,denm
-  spat,gnss,screen,cam_tx,cam,denm
+  spat,gnss_ubx,screen,cam_tx,cam,denm
 )
 features_test=(
   spat,cam,denm,uart

src/main.rs

@@ -8,11 +8,16 @@
 // #![deny(clippy::large_stack_frames)]
 
 // we can't use UART I/O and GNSS at the same time
-#[cfg(all(feature = "uart", feature = "gnss"))]
+#[cfg(all(feature = "uart", feature = "gnss_ubx", feature = "gnss_l67k"))]
-compile_error!("feature \"uart\" and feature \"gnss\" cannot be enabled at the same time");
+compile_error!(
+    "feature \"uart\" and features \"gnss_ubx\" or \"gnss_l67k\" cannot be enabled at the same time"
+);
 // autonomous CAM generation need a time and position source (aka _gnss)
 #[cfg(all(feature = "cam_tx", not(feature = "_gnss")))]
 compile_error!("feature \"cam_tx\" needs a time and position source (either GNSS or UART)");
+// we can't have both GNSS drivers enabled at the same time
+#[cfg(all(feature = "gnss_ubx", feature = "gnss_l67k"))]
+compile_error!("feature \"gnss_ubx\" and feature \"gnss_l67k\" cannot be enabled at the same time");
 
 use alloc::string::ToString as _;
 use alloc::vec::Vec;
@@ -26,7 +31,7 @@ use c_its_parser::standards::extensions::ItsStationType;
 use critical_section::Mutex;
 use crossbeam_queue::ArrayQueue;
 use embassy_executor::Spawner;
-#[cfg(feature = "gnss")]
+#[cfg(feature = "gnss_l67k")]
 use embassy_gps::gps::l76k;
 use esp_backtrace as _;
 use esp_hal::clock::CpuClock;
@@ -47,6 +52,8 @@ mod io;
 mod radio;
 #[cfg(feature = "screen")]
 mod screen;
+#[cfg(feature = "gnss_ubx")]
+mod ubx;
 #[cfg(any(feature = "cam_tx", feature = "uart"))]
 mod v2x_tx;
 
@@ -184,14 +191,15 @@ async fn main(spawner: Spawner) -> ! {
     let mac = esp_hal::efuse::base_mac_address();
     info!("WiFi MAC: {mac}");
 
-    // Setup GNSS: Seeed XIAO L67K:
+    // Setup GNSS:
+    // Seeed XIAO L67K:
     // RX:      D7/ GPIO12
     // TX:      D6/ GPIO11
     // WAKEUP:  D0/ GPIO1 -> HIGH for active, LOW for Sleep
     // RESET:   D2/ GPIO25 -> HIGH for normal, LOW for Reset
-    #[cfg(feature = "gnss")]
+    #[cfg(feature = "gnss_l67k")]
     spawner.spawn(
-        gnss_task(
+        gnss_l67k_task(
             peripherals.GPIO1,
             peripherals.GPIO25,
             peripherals.UART0,
@@ -200,7 +208,29 @@ async fn main(spawner: Spawner) -> ! {
         )
         .expect("Failed to spawn GNSS task"),
     );
-    #[cfg(feature = "gnss")]
+
+    // U-Blox module:
+    // RX:      D7/ GPIO12
+    // TX:      D6/ GPIO11
+    // RESET:   D2/ GPIO25 -> HIGH for normal, LOW for Reset
+    #[cfg(feature = "gnss_ubx")]
+    {
+        setup_uart0(
+            9600,
+            Some(b'\n'),
+            peripherals.GPIO12,
+            peripherals.GPIO11,
+            peripherals.UART0,
+            serial_handler,
+        )
+        .expect("Failed to initialize UART for GNSS");
+
+        // do some settings (UART receive and parsing runs in main loop)
+        spawner
+            .spawn(gnss_ubx_config(peripherals.GPIO25).expect("Failed to spawn GNSS config task"));
+    }
+
+    #[cfg(feature = "_gnss")]
     info!("GNSS task started, waiting for GNSS fix...");
 
     #[cfg(any(feature = "cam_tx", feature = "uart"))]
@@ -226,6 +256,8 @@ async fn main(spawner: Spawner) -> ! {
     };
     #[cfg(feature = "uart")]
     let mut uart_input = io::Parser::default();
+    #[cfg(feature = "gnss_ubx")]
+    let mut ubx_parser = ublox::Parser::default();
     loop {
         embassy_time::Timer::after(embassy_time::Duration::from_millis(1)).await;
 
@@ -290,6 +322,49 @@ async fn main(spawner: Spawner) -> ! {
                         }
                     }
                 }
+                #[cfg(feature = "gnss_ubx")]
+                {
+                    let mut it = ubx_parser.consume_ubx(&data);
+                    loop {
+                        match it.next() {
+                            Some(Ok(ublox::UbxPacket::Proto27(packet_ref))) => match &packet_ref {
+                                ublox::proto27::PacketRef::MonVer(packet) => {
+                                    println!("{}", ubx::pretty_print_mon_ver(packet));
+                                }
+                                ublox::proto27::PacketRef::MonComms(packet) => {
+                                    println!("{}", ubx::pretty_print_mon_comms(packet));
+                                }
+                                ublox::proto27::PacketRef::MonRf(packet) => {
+                                    println!("{}", ubx::pretty_print_mon_rf(packet));
+                                }
+                                ublox::proto27::PacketRef::NavSat(packet) => {
+                                    println!("{}", ubx::pretty_print_nav_sat(packet));
+                                }
+                                ublox::proto27::PacketRef::NavPvt(pvt) => {
+                                    match applogic::GnssFix::try_from(pvt) {
+                                        Ok(pos_state) => {
+                                            critical_section::with(|cs| {
+                                                let gnss_update_ref = GNSS_UPDATE.borrow(cs);
+                                                gnss_update_ref.replace(Some(pos_state));
+                                            });
+                                        }
+                                        Err(err) => warn!("{err}"),
+                                    }
+                                }
+                                _ => {
+                                    println!("UBX: {packet_ref:?}");
+                                }
+                            },
+                            Some(Err(_)) => {
+                                // Received a malformed packet
+                            }
+                            None => {
+                                // The internal buffer is now empty
+                                break;
+                            }
+                        }
+                    }
+                }
             }
         }
 
@@ -626,10 +701,10 @@ fn get_its_header<'p>(
     }
 }
 
-#[cfg(feature = "gnss")]
+#[cfg(feature = "gnss_l67k")]
 #[allow(clippy::large_stack_frames, reason = "GNSS driver needs some space")]
 #[embassy_executor::task]
-async fn gnss_task(
+async fn gnss_l67k_task(
     gps_wakeup: esp_hal::peripherals::GPIO1<'static>,
     gps_reset: esp_hal::peripherals::GPIO25<'static>,
     uart: esp_hal::peripherals::UART0<'static>,
@@ -679,6 +754,38 @@ async fn gnss_task(
     }
 }
 
+#[cfg(feature = "gnss_ubx")]
+#[embassy_executor::task]
+async fn gnss_ubx_config(reset_pin: esp_hal::peripherals::GPIO25<'static>) {
+    // reset the module
+    let mut gnss_reset = esp_hal::gpio::Output::new(
+        reset_pin,
+        esp_hal::gpio::Level::Low,
+        esp_hal::gpio::OutputConfig::default(),
+    );
+    embassy_time::Timer::after(embassy_time::Duration::from_millis(2)).await;
+    gnss_reset.set_high();
+
+    embassy_time::Timer::after(embassy_time::Duration::from_secs(1)).await;
+
+    info!("UBX: Enabling ubx protocol with NAV-PVT and NAV-SAT");
+    let buffer = ublox::packets::cfg_val::CfgValSetBuilder {
+        version: 1,
+        layers: ublox::packets::cfg_val::CfgLayerSet::RAM,
+        reserved1: 0,
+        cfg_data: &[
+            ublox::cfg_val::CfgVal::Uart1OutProtNmea(false),
+            ublox::cfg_val::CfgVal::Uart1OutProtUbx(true),
+            // CFG-MSGOUT-UBX_NAV_PVT_UART1
+            ublox::cfg_val::CfgVal::MsgOutUbxNavPvtUart1(1),
+            // CFG-MSGOUT-UBX_NAV_SAT_UART1
+            // ublox::cfg_val::CfgVal::MsgOutUbxNavSatUart1(10),
+        ],
+    }
+    .into_packet_vec();
+    send_uart0(&buffer);
+}
+
 #[cfg(feature = "screen")]
 fn log_screen_error<E>(error: &E)
 where

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),
+        })
+    }
+}