From 622eeeae82c200e6bb47b809b80717c072a0af50 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Sat, 20 Jun 2026 20:07:48 +0200 Subject: [PATCH 1/3] WIP v2x: drop hopped messages and unsupported message types TODO: Wait for c-its-parser release with updated GN (for Packet.btp_payload, etc.) TODO: Remove explicit GN dependency when feat/gn-pub-use is merged in c-its-parser --- Cargo.lock | 29 ++++++++++++----- Cargo.toml | 4 ++- src/main.rs | 93 +++++++++++++++++++++++++++++++++++++++++++++-------- 3 files changed, 103 insertions(+), 23 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 0b7e161..2a1246b 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -131,9 +131,9 @@ checksum = "c4512299f36f043ab09a583e57bceb5a5aab7a73db1805848e8fef3c9e8c78b3" [[package]] name = "bitvec" -version = "1.0.1" +version = "1.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1bc2832c24239b0141d5674bb9174f9d68a8b5b3f2753311927c172ca46f7e9c" +checksum = "ddcec3d12c579d40898fe0a9a358a803c23e9c52ca3c425707f81c9436211837" dependencies = [ "funty", "radium", @@ -186,14 +186,13 @@ checksum = "1e748733b7cbc798e1434b6ac524f0c1ff2ab456fe201501e6497c8417a4fc33" [[package]] name = "c-its-parser" -version = "2.2.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "993244dbda1234597ca13fe2e2565fc6f2ece585040404faa011482b55ed544e" +version = "2.2.4" +source = "git+https://github.com/consider-it/C-ITS-Parser.git?branch=feat%2Fgn-update#481dc7a92ee43a0aee5389762e15a17168019b3a" dependencies = [ "chrono", "etherparse", "geo-types", - "geonetworking", + "geonetworking 0.3.0-rc.6 (git+https://github.com/consider-it/geonetworking.git?branch=main)", "hex", "nom", "num-traits", @@ -1156,6 +1155,7 @@ dependencies = [ "esp-radio", "esp-rtos", "geo-types", + "geonetworking 0.3.0-rc.6 (registry+https://github.com/rust-lang/crates.io-index)", "log", "mipidsi", "num-traits", @@ -1347,9 +1347,22 @@ dependencies = [ [[package]] name = "geonetworking" -version = "0.2.1" +version = "0.3.0-rc.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ea4f48e7b1cdf91ad77ec1e4c5617aee3666a929288f4a3f11e473b67d3a6314" +checksum = "c60eaa1367ffa087fa9a5c2c9f32578290a808232189e811746d84fcde01e479" +dependencies = [ + "bitvec", + "bitvec-nom2", + "bytes", + "nom", + "num", + "num-traits", +] + +[[package]] +name = "geonetworking" +version = "0.3.0-rc.6" +source = "git+https://github.com/consider-it/geonetworking.git?branch=main#fd97d28f547355e008caced31271361ed2893405" dependencies = [ "bitvec", "bitvec-nom2", diff --git a/Cargo.toml b/Cargo.toml index 3eddcc6..9976afe 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -85,7 +85,8 @@ esp-radio = { version = "0.18.0", optional = true, features = [ embassy_gps = { version = "0.1.0", optional = true, default-features = false, features = ["esp", "log-04"], git = "https://github.com/jbeyerstedt/embassy_gps.git", branch = "feat/rework-gpsfix" } embedded-hal-bus = { version = "0.3.0", optional = true } embedded-graphics = { version = "0.8.2", optional = true } -c-its-parser = { version = "2.2.4", default-features = false, features = [ +# needs newer GN version +c-its-parser = { version = "2.2.4", default-features = false, branch = "feat/gn-update", git = "https://github.com/consider-it/C-ITS-Parser.git", features = [ "time", "geo", "libm", @@ -94,6 +95,7 @@ c-its-parser = { version = "2.2.4", default-features = false, features = [ chrono = { version = "0.4.44", default-features = false, features = ["alloc"] } crossbeam-queue = { version = "0.3.12", default-features = false, features = ["alloc"] } geo-types = { version = "0.7.19", default-features = false } +geonetworking = { version = "0.3.0-rc.5", default-features = false } 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 } diff --git a/src/main.rs b/src/main.rs index 1a5d2ef..91b7fc3 100644 --- a/src/main.rs +++ b/src/main.rs @@ -7,6 +7,7 @@ )] // #![deny(clippy::large_stack_frames)] +use alloc::string::ToString as _; use alloc::vec::Vec; use core::cell::RefCell; @@ -23,6 +24,8 @@ use esp_hal::{delay, gpio, spi, time}; #[cfg(all(target_arch = "riscv32", feature = "spat"))] use esp_println::println; use esp_radio::wifi; +// use c_its_parser::gn as geonetworking; +use geonetworking::Decode as _; use log::{debug, error, info, warn}; extern crate alloc; @@ -227,12 +230,12 @@ async fn main(spawner: Spawner) -> ! { } }); if let Some(data) = new_data { - match c_its_parser::de::decode(&data, c_its_parser::Headers::IEEE802LlcGnBtp) { + match c_its_parser::de::decode(&data, c_its_parser::Headers::None) { Ok(msg) => { use c_its_parser::ItsMessage; - use c_its_parser::standards::extensions::ItsMessageId; + // use c_its_parser::standards::extensions::ItsMessageId; - debug!("Got new {:?} message", ItsMessageId::from(&msg)); + // debug!("Got new {:?} message", ItsMessageId::from(&msg)); match msg { #[cfg(feature = "spat")] @@ -276,8 +279,9 @@ async fn main(spawner: Spawner) -> ! { } } Err(err) => { - // will give false-positives when a message is received which wasn't enabled - debug!("Failed to parse V2X message: {err}"); + // we already filtered out all unsupported message types in the wifi rx callback + // so these are actual errors + warn!("Failed to parse V2X message: {err}"); } } } @@ -319,16 +323,77 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) { debug!("Received frame with {} bytes", frame.len); - // Send data to main thread - critical_section::with(|cs| { - let queue_rc = WIFI_RX_QUEUE.borrow(cs).borrow(); - // unwrap is fine b/c we stored something in it before - let queue = queue_rc.as_ref().unwrap(); - - if queue.push(frame.data.to_vec()).is_err() { - error!("V2X RX queue is full"); + // parse GN headers + match c_its_parser::pcap::remove_wlan_headers(frame.data) + .map_err(|err| alloc::format!("Failed to parse WLAN headers: {err}")) + .and_then(|data| { + geonetworking::Packet::decode(data) + .map_err(|err| alloc::format!("Failed to parse GN headers: {err:?}")) + }) { + Err(err) => { + warn!("{err}"); } - }); + Ok(packet) => { + // drop hopped (for now) + if packet.decoded.is_hopped() { + return; + } + + // "parse" ITS PDU Header + if let Ok((payload, message_id, _)) = get_its_header(&packet) { + // drop all unsupported message IDs + if match message_id { + #[cfg(feature = "denm")] + c_its_parser::standards::extensions::ItsMessageId::Denm => false, + #[cfg(feature = "cam")] + c_its_parser::standards::extensions::ItsMessageId::Cam => false, + #[cfg(feature = "spat")] + c_its_parser::standards::extensions::ItsMessageId::Spatem + | c_its_parser::standards::extensions::ItsMessageId::Mapem => false, + _ => true, + } { + return; + } + + // Send data to main thread + critical_section::with(|cs| { + let queue_rc = WIFI_RX_QUEUE.borrow(cs).borrow(); + // unwrap is fine b/c we stored something in it before + let queue = queue_rc.as_ref().unwrap(); + + if queue.push(payload.to_vec()).is_err() { + error!("V2X RX queue is full"); + } + }); + } + } + } +} + +fn get_its_header<'p>( + packet: &geonetworking::Decoded>, +) -> Result< + ( + &'p [u8], + c_its_parser::standards::extensions::ItsMessageId, + u32, + ), + alloc::string::String, +> { + let payload = packet.decoded.btp_payload()?; + + if payload.len() < 6 { + return Err("ITS payload too small for ItsPduHeader".to_string()); + } + + if let Ok(message_id) = payload[1].try_into() { + let station_id_buf: [u8; 4] = payload[2..6].try_into().unwrap(); + let station_id = u32::from_be_bytes(station_id_buf); + + Ok((payload, message_id, station_id)) + } else { + Err("Unknown message ID in ItsPduHeader".to_string()) + } } #[cfg(feature = "gnss")] From e7493efcc182e196dfcb380e2485cdbbbbadd169 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Sat, 20 Jun 2026 20:15:11 +0200 Subject: [PATCH 2/3] v2x: Reduce SPAT rate --- src/main.rs | 89 ++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 84 insertions(+), 5 deletions(-) diff --git a/src/main.rs b/src/main.rs index 91b7fc3..23ed774 100644 --- a/src/main.rs +++ b/src/main.rs @@ -10,6 +10,8 @@ use alloc::string::ToString as _; use alloc::vec::Vec; use core::cell::RefCell; +#[cfg(feature = "spat")] +use core::cell::UnsafeCell; use critical_section::Mutex; use crossbeam_queue::ArrayQueue; @@ -43,6 +45,8 @@ const WIFI_CHANNEL: radio::Channel = 180; pub const DISPLAY_WIDTH: u16 = 320; #[cfg(feature = "screen")] pub const DISPLAY_HEIGHT: u16 = 172; +#[cfg(feature = "spat")] +const SPAT_RATE_LIMIT: u8 = 5; // keep every n-th message // This creates a default app-descriptor required by the esp-idf bootloader. // For more information see: @@ -235,7 +239,7 @@ async fn main(spawner: Spawner) -> ! { use c_its_parser::ItsMessage; // use c_its_parser::standards::extensions::ItsMessageId; - // debug!("Got new {:?} message", ItsMessageId::from(&msg)); + // info!("Got new {:?} message", ItsMessageId::from(&msg)); match msg { #[cfg(feature = "spat")] @@ -295,7 +299,28 @@ async fn main(spawner: Spawner) -> ! { clippy::needless_pass_by_value, reason = "adhering to callback interface" )] +#[allow(clippy::too_many_lines)] fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) { + #[cfg(feature = "spat")] + const PRUNE_INTERVAL: chrono::Duration = chrono::Duration::seconds(60); + #[cfg(feature = "spat")] + static mut RATELIMIT_CACHE_LAST_PRUNE: UnsafeCell = + UnsafeCell::new(chrono::NaiveDateTime::MIN); + + #[cfg(feature = "spat")] + static mut SPAT_RATELIMIT_CACHE: UnsafeCell> = UnsafeCell::new( + cache::Cache::::new(chrono::Duration::seconds(20)), + ); + + #[cfg(feature = "spat")] + let now = { + let time = esp_hal::time::Instant::now().duration_since_epoch(); + // unwrap is fine since value range is u64 microseconds + chrono::DateTime::from_timestamp(time.as_secs().cast_signed(), 0) + .unwrap() + .naive_utc() + }; + // Ignore frames with errors and non-data frames if frame.rx_cntl.rx_state != 0 { warn!("Received frame has RX error: {}", frame.rx_cntl.rx_state); @@ -340,16 +365,37 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) { } // "parse" ITS PDU Header - if let Ok((payload, message_id, _)) = get_its_header(&packet) { - // drop all unsupported message IDs + if let Ok((payload, message_id, station_id)) = get_its_header(&packet) { + // drop all unsupported message IDs and rate-limit SPATEMs if match message_id { #[cfg(feature = "denm")] c_its_parser::standards::extensions::ItsMessageId::Denm => false, #[cfg(feature = "cam")] c_its_parser::standards::extensions::ItsMessageId::Cam => false, #[cfg(feature = "spat")] - c_its_parser::standards::extensions::ItsMessageId::Spatem - | c_its_parser::standards::extensions::ItsMessageId::Mapem => false, + c_its_parser::standards::extensions::ItsMessageId::Spatem => { + // reduce SPAT rate per station ID + let mut drop_msg = false; + unsafe { + #[allow( + static_mut_refs, + reason = "this function will only run consecutive" + )] + let spat_cache = SPAT_RATELIMIT_CACHE.get(); + + (*spat_cache).update_or_init(now, station_id, |i| { + i.msg_count = i.msg_count.wrapping_add(1); + + if (i.msg_count % SPAT_RATE_LIMIT) > 0 { + drop_msg = true; + } + }); + } + + drop_msg + } + #[cfg(feature = "spat")] + c_its_parser::standards::extensions::ItsMessageId::Mapem => false, _ => true, } { return; @@ -368,6 +414,39 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) { } } } + + #[cfg(feature = "spat")] + unsafe { + #[allow(static_mut_refs, reason = "this function will only run consecutive")] + let last_prune_time = RATELIMIT_CACHE_LAST_PRUNE.get(); + if *last_prune_time + PRUNE_INTERVAL < now { + #[allow(static_mut_refs, reason = "this function will only run consecutive")] + let spat_cache = SPAT_RATELIMIT_CACHE.get(); + (*spat_cache).prune(now); + + *last_prune_time = now; + } + } +} + +#[derive(Debug, Default, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] +struct RatelimitData { + station_id: u32, + msg_count: u8, +} + +impl cache::Cachable for RatelimitData { + fn key(&self) -> u32 { + self.station_id + } +} +impl cache::Initable for RatelimitData { + fn init(key: u32) -> Self { + Self { + station_id: key, + ..Default::default() + } + } } fn get_its_header<'p>( From 8097937cdb9a5b1b6457241c2ede3d3c87f16f48 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 22 Jun 2026 22:57:48 +0200 Subject: [PATCH 3/3] WIP: try to debug V2X rx dropouts --- src/main.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main.rs b/src/main.rs index 23ed774..d1603f3 100644 --- a/src/main.rs +++ b/src/main.rs @@ -321,6 +321,8 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) { .naive_utc() }; + esp_println::print!("."); // TODO: debug only! + // Ignore frames with errors and non-data frames if frame.rx_cntl.rx_state != 0 { warn!("Received frame has RX error: {}", frame.rx_cntl.rx_state);