From 622eeeae82c200e6bb47b809b80717c072a0af50 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Sat, 20 Jun 2026 20:07:48 +0200 Subject: [PATCH] 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")]