From 2db0dc01163da34744336a2cb4b3b7bca9166b17 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Sun, 26 Jul 2026 21:34:18 +0200 Subject: [PATCH] gnss: Add support for ublox GNSS --- Cargo.lock | 24 ++++++++++ Cargo.toml | 7 ++- Readme.md | 13 ++--- feat-permut.sh | 8 ++-- src/main.rs | 125 +++++++++++++++++++++++++++++++++++++++++++++---- src/ubx.rs | 106 +++++++++++++++++++++++++++++++++++++++++ 6 files changed, 262 insertions(+), 21 deletions(-) create mode 100644 src/ubx.rs diff --git a/Cargo.lock b/Cargo.lock index 3fba778..b3d5c4c 100644 --- a/Cargo.lock +++ b/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" diff --git a/Cargo.toml b/Cargo.toml index eef57d9..55f42c5 100644 --- a/Cargo.toml +++ b/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" diff --git a/Readme.md b/Readme.md index ace338e..2ae3aab 100644 --- a/Readme.md +++ b/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)) -only available with `gnss` feature (which is included in `spat` feature) +- U-Blox M10Q GNSS (connected on GPIO12 (RX), GPIO11 (TX), and GPIO25 (reset)) +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,screen,cam_tx` ("standalone with CAM") -- `esp,spat,gnss,screen,cam_tx,cam,denm` +- `esp,spat,gnss_ubx,screen` (default features, "standalone") +- `esp,spat,gnss_ubx,screen,cam_tx` ("standalone with CAM") +- `esp,spat,gnss_ubx,screen,cam_tx,cam,denm` - `esp,uart,cam,denm` ("smartphone companion") Connect your ESP32-C5 and run: diff --git a/feat-permut.sh b/feat-permut.sh index 2019bef..9c08d65 100755 --- a/feat-permut.sh +++ b/feat-permut.sh @@ -9,12 +9,12 @@ extended=${2:-false} features_base=( uart,cam,denm - spat,gnss,screen - spat,gnss,screen,cam_tx + spat,gnss_ubx,screen + spat,gnss_ubx,screen,cam_tx ) features_more=( - spat,gnss,screen,cam,denm - spat,gnss,screen,cam_tx,cam,denm + spat,gnss_ubx,screen,cam,denm + spat,gnss_ubx,screen,cam_tx,cam,denm ) features_test=( spat,cam,denm,uart diff --git a/src/main.rs b/src/main.rs index 64ed528..5320951 100644 --- a/src/main.rs +++ b/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"))] -compile_error!("feature \"uart\" and feature \"gnss\" cannot be enabled at the same time"); +#[cfg(all(feature = "uart", feature = "gnss_ubx", feature = "gnss_l67k"))] +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(error: &E) where diff --git a/src/ubx.rs b/src/ubx.rs new file mode 100644 index 0000000..934af7b --- /dev/null +++ b/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::>() + ) +} + +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::>() + ) +} + +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::>() + ) +} + +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::>(); + 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::>(); + + 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 { + 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 = 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), + }) + } +}