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

v2x: drop hopped messages and unsupported message types

Browse source
This commit is contained in:
Jannik Beyerstedt 2026-06-20 20:07:48 +02:00 • committed by Jannik Beyerstedt
commit e178c740db
3 changed files with 85 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

12
Cargo.lock generated
View file

@ -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,9 +186,9 @@ checksum = "1e748733b7cbc798e1434b6ac524f0c1ff2ab456fe201501e6497c8417a4fc33"
[[package]]
name = "c-its-parser"
version = "2.2.5"
version = "2.3.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "993244dbda1234597ca13fe2e2565fc6f2ece585040404faa011482b55ed544e"
checksum = "f9bd739e1f76c017fc9fc6c21b9ff2aee4536bef357075d454e387e94a4d6287"
dependencies = [
"chrono",
"etherparse",
@ -1347,9 +1347,9 @@ dependencies = [
[[package]]
name = "geonetworking"
version = "0.2.1"
version = "0.3.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "ea4f48e7b1cdf91ad77ec1e4c5617aee3666a929288f4a3f11e473b67d3a6314"
checksum = "6f6d36111f5057d3be7eb4e312a07edac4737fc791e4b0a73d812c6f710161e7"
dependencies = [
"bitvec",
"bitvec-nom2",

2
Cargo.toml
View file

@ -85,7 +85,7 @@ 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 = [
c-its-parser = { version = "2.3.0", default-features = false, features = [
"time",
"geo",
"libm",

77
src/main.rs
View file

@ -7,9 +7,11 @@
)]
// #![deny(clippy::large_stack_frames)]
use alloc::string::ToString as _;
use alloc::vec::Vec;
use core::cell::RefCell;
use c_its_parser::gn as geonetworking;
use critical_section::Mutex;
use crossbeam_queue::ArrayQueue;
use embassy_executor::Spawner;
@ -21,6 +23,7 @@ use esp_hal::timer::timg::TimerGroup;
#[cfg(all(target_arch = "riscv32", feature = "spat"))]
use esp_println::println;
use esp_radio::wifi;
use geonetworking::Decode as _;
use log::{debug, error, info, warn};
extern crate alloc;
@ -204,12 +207,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")]
@ -253,8 +256,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}");
}
}
}
@ -294,17 +298,78 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
// info!("Received frame with {} bytes", frame.len);
// 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(frame.data.to_vec()).is_err() {
if queue.push(payload.to_vec()).is_err() {
error!("V2X RX queue is full");
}
});
}
}
}
}
fn get_its_header<'p>(
packet: &geonetworking::Decoded<geonetworking::Packet<'p>>,
) -> 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")]
#[allow(clippy::large_stack_frames, reason = "GNSS driver needs some space")]