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
This commit is contained in:
parent
3ae65edcbe
commit
622eeeae82
3 changed files with 102 additions and 22 deletions
29
Cargo.lock
generated
29
Cargo.lock
generated
|
|
@ -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",
|
||||
|
|
|
|||
|
|
@ -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 }
|
||||
|
|
|
|||
93
src/main.rs
93
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<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")]
|
||||
|
|
|
|||
Loading…
Reference in a new issue