jtbx/esp32-c_its-companion
0
0
Fork 0
Code Activity

gnss: Add support for ublox GNSS

Browse source
This commit is contained in:
Jannik Beyerstedt 2026-07-26 21:34:18 +02:00
commit 2db0dc0116
6 changed files with 262 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

125
src/main.rs
View file

@ -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<E>(error: &E)
where

106
src/ubx.rs Normal file
View file

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