feat: Add some message parsing
This commit is contained in:
parent
919065f377
commit
9bf8b8e4de
8 changed files with 1006 additions and 6 deletions
65
src/applogic/cam.rs
Normal file
65
src/applogic/cam.rs
Normal file
|
|
@ -0,0 +1,65 @@
|
|||
//! C-ITS CAM Evaluation
|
||||
|
||||
use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::{self, HighFrequencyContainer};
|
||||
use log::info;
|
||||
|
||||
pub fn handle_cam(etsi: &cam_pdu_descriptions::CAM) {
|
||||
// only POC output
|
||||
match &etsi.cam.cam_parameters.high_frequency_container {
|
||||
HighFrequencyContainer::basicVehicleContainerHighFrequency(obu) => {
|
||||
let station_id = etsi.header.station_id.0;
|
||||
let lat_deg = etsi
|
||||
.cam
|
||||
.cam_parameters
|
||||
.basic_container
|
||||
.reference_position
|
||||
.latitude
|
||||
.as_deg();
|
||||
let lon_deg = etsi
|
||||
.cam
|
||||
.cam_parameters
|
||||
.basic_container
|
||||
.reference_position
|
||||
.longitude
|
||||
.as_deg();
|
||||
|
||||
if obu.speed.speed_value.is_unavailable() {
|
||||
info!(
|
||||
"Got CAM from {station_id} at {lat_deg:.3}°N, {lon_deg:.3}°E, speed unavailable",
|
||||
);
|
||||
} else {
|
||||
info!(
|
||||
"Got CAM from {station_id} at {lat_deg:.3}°N, {lon_deg:.3}°E, speed {:.2} km/h",
|
||||
obu.speed.speed_value.as_kmh()
|
||||
);
|
||||
}
|
||||
}
|
||||
HighFrequencyContainer::rsuContainerHighFrequency(rsu) => {
|
||||
info!(
|
||||
"Got RSU from {} at {:.3}°N, {:.3}°E, {} protected zones",
|
||||
etsi.header.station_id.0,
|
||||
etsi.cam
|
||||
.cam_parameters
|
||||
.basic_container
|
||||
.reference_position
|
||||
.latitude
|
||||
.as_deg(),
|
||||
etsi.cam
|
||||
.cam_parameters
|
||||
.basic_container
|
||||
.reference_position
|
||||
.longitude
|
||||
.as_deg(),
|
||||
rsu.protected_communication_zones_rsu
|
||||
.as_ref()
|
||||
.map(|d| d.0.len())
|
||||
.unwrap_or_default()
|
||||
);
|
||||
}
|
||||
|
||||
_ => {
|
||||
// enum is non-exhaustive, so we need a default case
|
||||
// but this should never be reached in pactical applications
|
||||
}
|
||||
}
|
||||
}
|
||||
182
src/applogic/denm.rs
Normal file
182
src/applogic/denm.rs
Normal file
|
|
@ -0,0 +1,182 @@
|
|||
//! C-ITS DENM Evaluation
|
||||
|
||||
use c_its_parser::standards::cdd_2_2_1::etsi_its_cdd;
|
||||
use c_its_parser::standards::denm_2_2_1::denm_pdu_description;
|
||||
use log::info;
|
||||
|
||||
pub fn handle_denm(etsi: &denm_pdu_description::DENM) {
|
||||
let mgmt = &etsi.denm.management;
|
||||
|
||||
if mgmt.termination.is_some() {
|
||||
return;
|
||||
}
|
||||
|
||||
if let (Some(sit), Some(loc)) = (&etsi.denm.situation, &etsi.denm.location) {
|
||||
// let timestamp: chrono::DateTime<chrono::Utc> = mgmt.reference_time.clone().into();
|
||||
let end_time = chrono::DateTime::<chrono::Utc>::from(mgmt.reference_time.clone())
|
||||
+ chrono::Duration::seconds(mgmt.validity_duration.0.into());
|
||||
let station_id = &mgmt.action_id.originating_station_id.0;
|
||||
let seq_num = mgmt.action_id.sequence_number.0;
|
||||
|
||||
let lat_deg = mgmt.event_position.latitude.as_deg();
|
||||
let lon_deg = mgmt.event_position.longitude.as_deg();
|
||||
|
||||
// TODO: use `as_deg()` when in new c-its-parser release
|
||||
let heading = loc
|
||||
.event_position_heading
|
||||
.as_ref()
|
||||
.map(|v| f32::from(v.value.0) / 10f32);
|
||||
|
||||
// let direction = mgmt.traffic_direction;
|
||||
|
||||
let (cc, scc) = cause_code_choice_to_numbers(&sit.event_type.cc_and_scc);
|
||||
|
||||
if let Some(hdg) = heading {
|
||||
info!(
|
||||
"DENN: {station_id}/{seq_num} CC {cc}/{scc} at {lat_deg:.3}°N, {lon_deg:.3}°E, hdg {hdg:.1}° until {end_time:?}"
|
||||
)
|
||||
} else {
|
||||
info!(
|
||||
"DENN: {station_id}/{seq_num} CC {cc}/{scc} at {lat_deg:.3}°N, {lon_deg:.3}°E until {end_time:?}"
|
||||
)
|
||||
}
|
||||
} else {
|
||||
info!("DENM: One of situation or location container not available");
|
||||
}
|
||||
}
|
||||
|
||||
fn cause_code_choice_to_numbers(choice: &etsi_its_cdd::CauseCodeChoice) -> (u8, u8) {
|
||||
match choice {
|
||||
etsi_its_cdd::CauseCodeChoice::reserved0(scc) => (0, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::trafficCondition1(scc) => (1, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::accident2(scc) => (2, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::roadworks3(scc) => (3, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved4(scc) => (4, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::impassability5(scc) => (5, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::adverseWeatherCondition_Adhesion6(scc) => (6, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::aquaplaning7(scc) => (7, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved8(scc) => (8, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::hazardousLocation_SurfaceCondition9(scc) => (9, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::hazardousLocation_ObstacleOnTheRoad10(scc) => (10, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::hazardousLocation_AnimalOnTheRoad11(scc) => (11, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::humanPresenceOnTheRoad12(scc) => (12, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved13(scc) => (13, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::wrongWayDriving14(scc) => (14, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::rescueAndRecoveryWorkInProgress15(scc) => (15, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved16(scc) => (16, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::adverseWeatherCondition_ExtremeWeatherCondition17(scc) => {
|
||||
(17, scc.0)
|
||||
}
|
||||
etsi_its_cdd::CauseCodeChoice::adverseWeatherCondition_Visibility18(scc) => (18, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::adverseWeatherCondition_Precipitation19(scc) => (19, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::violence20(scc) => (20, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved21(scc) => (21, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved22(scc) => (22, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved23(scc) => (23, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved24(scc) => (24, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved25(scc) => (25, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::slowVehicle26(scc) => (26, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::dangerousEndOfQueue27(scc) => (27, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::publicTransportVehicleApproaching28(scc) => (28, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved29(scc) => (29, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved30(scc) => (30, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved31(scc) => (31, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved32(scc) => (32, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved33(scc) => (33, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved34(scc) => (34, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved35(scc) => (35, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved36(scc) => (36, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved37(scc) => (37, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved38(scc) => (38, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved39(scc) => (39, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved40(scc) => (40, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved41(scc) => (41, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved42(scc) => (42, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved43(scc) => (43, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved44(scc) => (44, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved45(scc) => (45, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved46(scc) => (46, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved47(scc) => (47, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved48(scc) => (48, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved49(scc) => (49, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved50(scc) => (50, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved51(scc) => (51, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved52(scc) => (52, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved53(scc) => (53, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved54(scc) => (54, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved55(scc) => (55, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved56(scc) => (56, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved57(scc) => (57, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved58(scc) => (58, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved59(scc) => (59, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved60(scc) => (60, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved61(scc) => (61, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved62(scc) => (62, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved63(scc) => (63, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved64(scc) => (64, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved65(scc) => (65, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved66(scc) => (66, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved67(scc) => (67, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved68(scc) => (68, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved69(scc) => (69, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved70(scc) => (70, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved71(scc) => (71, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved72(scc) => (72, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved73(scc) => (73, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved74(scc) => (74, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved75(scc) => (75, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved76(scc) => (76, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved77(scc) => (77, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved78(scc) => (78, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved79(scc) => (79, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved80(scc) => (80, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved81(scc) => (81, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved82(scc) => (82, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved83(scc) => (83, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved84(scc) => (84, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved85(scc) => (85, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved86(scc) => (86, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved87(scc) => (87, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved88(scc) => (88, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved89(scc) => (89, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved90(scc) => (90, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::vehicleBreakdown91(scc) => (91, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::postCrash92(scc) => (92, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::humanProblem93(scc) => (93, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::stationaryVehicle94(scc) => (94, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::emergencyVehicleApproaching95(scc) => (95, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::hazardousLocation_DangerousCurve96(scc) => (96, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::collisionRisk97(scc) => (97, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::signalViolation98(scc) => (98, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::dangerousSituation99(scc) => (99, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::railwayLevelCrossing100(scc) => (100, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved101(scc) => (101, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved102(scc) => (102, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved103(scc) => (103, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved104(scc) => (104, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved105(scc) => (105, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved106(scc) => (106, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved107(scc) => (107, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved108(scc) => (108, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved109(scc) => (109, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved110(scc) => (110, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved111(scc) => (111, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved112(scc) => (112, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved113(scc) => (113, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved114(scc) => (114, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved115(scc) => (115, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved116(scc) => (116, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved117(scc) => (117, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved118(scc) => (118, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved119(scc) => (119, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved120(scc) => (120, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved121(scc) => (121, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved122(scc) => (122, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved123(scc) => (123, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved124(scc) => (124, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved125(scc) => (125, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved126(scc) => (126, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved127(scc) => (127, scc.0),
|
||||
etsi_its_cdd::CauseCodeChoice::reserved128(scc) => (128, scc.0),
|
||||
}
|
||||
}
|
||||
8
src/applogic/mod.rs
Normal file
8
src/applogic/mod.rs
Normal file
|
|
@ -0,0 +1,8 @@
|
|||
//! Application Logic
|
||||
|
||||
#[cfg(feature = "cam")]
|
||||
pub mod cam;
|
||||
#[cfg(feature = "denm")]
|
||||
pub mod denm;
|
||||
#[cfg(feature = "spat")]
|
||||
pub mod tlc;
|
||||
51
src/applogic/tlc.rs
Normal file
51
src/applogic/tlc.rs
Normal file
|
|
@ -0,0 +1,51 @@
|
|||
//! C-ITS Traffic Light Data (MAPEM/ SPATEM) Evaluation
|
||||
|
||||
use c_its_parser::standards::mapem_2_2_1::mapem_pdu_descriptions;
|
||||
use c_its_parser::standards::spatem_2_2_1::spatem_pdu_descriptions;
|
||||
use chrono::Datelike;
|
||||
use esp_println::println;
|
||||
|
||||
pub fn handle_mapem(etsi: &mapem_pdu_descriptions::MAPEM) {
|
||||
// TODO: proof of concept only -- replace with actual evaluation
|
||||
if let Some(map) = &etsi.map.intersections {
|
||||
for int in &map.0 {
|
||||
println!("got MAPEM for intersection {}", int.id.id.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn handle_spatem(etsi: &spatem_pdu_descriptions::SPATEM, now: &chrono::NaiveDateTime) {
|
||||
// TODO: proof of concept only -- replace with actual evaluation
|
||||
for int in &etsi.spat.intersections.0 {
|
||||
println!("got SPATEM for intersection {}", int.id.id.0);
|
||||
|
||||
if let (Some(moy), Some(second)) = (&int.moy, &int.time_stamp) {
|
||||
let year = now.year();
|
||||
|
||||
let ref_time = c_its_parser::time_utils::time_from_moy_and_dsecond(moy, second, year);
|
||||
println!("- ref time: {ref_time:?}");
|
||||
|
||||
for state in &int.states.0 {
|
||||
let sig_grp = state.signal_group.0;
|
||||
|
||||
if let Some(event) = state.state_time_speed.0.first() {
|
||||
if let Some(timing) = &event.timing {
|
||||
let min_end_time = timing.min_end_time.to_datetime_from_moy(moy, year);
|
||||
|
||||
println!(
|
||||
"- sig grp. {sig_grp:03}: until {min_end_time:?} at {:?}",
|
||||
event.event_state
|
||||
);
|
||||
} else {
|
||||
println!(
|
||||
"- sig grp. {sig_grp:03}: {:?} until unknown",
|
||||
event.event_state
|
||||
);
|
||||
}
|
||||
} else {
|
||||
println!("- sig grp. {sig_grp:03}: no events");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
103
src/main.rs
103
src/main.rs
|
|
@ -7,19 +7,22 @@
|
|||
)]
|
||||
// #![deny(clippy::large_stack_frames)]
|
||||
|
||||
use alloc::vec::Vec;
|
||||
use core::cell::RefCell;
|
||||
|
||||
use critical_section::Mutex;
|
||||
use crossbeam_queue::ArrayQueue;
|
||||
use embassy_executor::Spawner;
|
||||
use embassy_gps::gps::l76k;
|
||||
use esp_backtrace as _;
|
||||
use esp_hal::clock::CpuClock;
|
||||
use esp_hal::timer::timg::TimerGroup;
|
||||
use esp_radio::wifi;
|
||||
use log::{debug, info, warn};
|
||||
use log::{debug, error, info, warn};
|
||||
|
||||
extern crate alloc;
|
||||
|
||||
mod applogic;
|
||||
mod radio;
|
||||
|
||||
const WIFI_CHANNEL: radio::Channel = 180;
|
||||
|
|
@ -85,6 +88,7 @@ esp_bootloader_esp_idf::esp_app_desc!();
|
|||
|
||||
static GNSS_UPDATE: Mutex<RefCell<Option<embassy_gps::types::GpsFix>>> =
|
||||
Mutex::new(RefCell::new(None));
|
||||
static WIFI_RX_QUEUE: Mutex<RefCell<Option<ArrayQueue<Vec<u8>>>>> = Mutex::new(RefCell::new(None));
|
||||
|
||||
#[allow(
|
||||
clippy::large_stack_frames,
|
||||
|
|
@ -111,6 +115,7 @@ async fn main(spawner: Spawner) -> ! {
|
|||
// - GPIO28
|
||||
|
||||
esp_alloc::heap_allocator!(#[esp_hal::ram(reclaimed)] size: 65536);
|
||||
esp_alloc::psram_allocator!(peripherals.PSRAM, esp_hal::psram);
|
||||
|
||||
let timg0 = TimerGroup::new(peripherals.TIMG0);
|
||||
let sw_interrupt =
|
||||
|
|
@ -125,6 +130,10 @@ async fn main(spawner: Spawner) -> ! {
|
|||
)
|
||||
.expect("Failed to initialize Wi-Fi controller");
|
||||
|
||||
critical_section::with(|cs| {
|
||||
WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2)));
|
||||
});
|
||||
|
||||
let mut sniffer = interfaces.sniffer;
|
||||
radio::setup_wifi_sniffer(WIFI_CHANNEL, &mut sniffer, handle_frame)
|
||||
.expect("Fatal error initializing 802.11p sniffer");
|
||||
|
|
@ -150,7 +159,7 @@ async fn main(spawner: Spawner) -> ! {
|
|||
|
||||
let mut state = State::default();
|
||||
loop {
|
||||
embassy_time::Timer::after(embassy_time::Duration::from_millis(100)).await;
|
||||
embassy_time::Timer::after(embassy_time::Duration::from_millis(5)).await;
|
||||
|
||||
critical_section::with(|cs| {
|
||||
let gnss_update_ref = GNSS_UPDATE.borrow(cs);
|
||||
|
|
@ -164,6 +173,68 @@ async fn main(spawner: Spawner) -> ! {
|
|||
}
|
||||
}
|
||||
});
|
||||
|
||||
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 let Some(data) = queue.pop() {
|
||||
match c_its_parser::de::decode(&data, c_its_parser::Headers::IEEE802LlcGnBtp) {
|
||||
Ok(msg) => {
|
||||
use c_its_parser::ItsMessage;
|
||||
use c_its_parser::standards::extensions::ItsMessageId;
|
||||
|
||||
debug!("Got new {:?} message", ItsMessageId::from(&msg));
|
||||
|
||||
match msg {
|
||||
#[cfg(feature = "spat")]
|
||||
ItsMessage::Mapem {
|
||||
geonetworking: _,
|
||||
transport: _,
|
||||
etsi,
|
||||
} => applogic::tlc::handle_mapem(&etsi),
|
||||
#[cfg(feature = "spat")]
|
||||
ItsMessage::Spatem {
|
||||
geonetworking: _,
|
||||
transport: _,
|
||||
etsi,
|
||||
} => {
|
||||
if state.initialized {
|
||||
applogic::tlc::handle_spatem(&etsi, &state.time);
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "cam")]
|
||||
ItsMessage::Cam {
|
||||
geonetworking: _,
|
||||
transport: _,
|
||||
etsi,
|
||||
} => applogic::cam::handle_cam(&etsi),
|
||||
|
||||
#[cfg(feature = "denm")]
|
||||
ItsMessage::DenmV2 {
|
||||
geonetworking: _,
|
||||
transport: _,
|
||||
etsi,
|
||||
} => applogic::denm::handle_denm(&etsi),
|
||||
#[cfg(feature = "denm")]
|
||||
ItsMessage::DenmV1 {
|
||||
geonetworking: _,
|
||||
transport: _,
|
||||
etsi: _,
|
||||
} => {
|
||||
// irrelevant since all received DENMs should be parsed as v2
|
||||
}
|
||||
}
|
||||
}
|
||||
Err(err) => {
|
||||
// will give false-positives when a message is received which wasn't enabled
|
||||
debug!("Failed to parse V2X message: {err}");
|
||||
}
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -183,8 +254,32 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
|
|||
return;
|
||||
}
|
||||
|
||||
// TODO: handle 802.11 frame
|
||||
info!("Received frame with {} bytes", frame.len);
|
||||
// Ignore non-broadcast frames
|
||||
if frame.len < (4 + 6) {
|
||||
warn!(
|
||||
"Received frame too small to be a valid broadcast frame: {} bytes",
|
||||
frame.len
|
||||
);
|
||||
return;
|
||||
}
|
||||
let dest_mac = &frame.data[4..10];
|
||||
if dest_mac != [0xff; 6] {
|
||||
warn!("Received frame is not broadcast frame: Dest MAC {dest_mac:02x?}");
|
||||
return;
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
#[allow(clippy::large_stack_frames, reason = "GNSS driver needs some space")]
|
||||
|
|
|
|||
|
|
@ -67,7 +67,7 @@ impl PromiscuousPktType {
|
|||
core::ffi::c_uint::from(self.as_repr())
|
||||
}
|
||||
|
||||
/// Determines if
|
||||
/// Determines if we are the same frame type as `esp_radio::wifi::sniffer::PromiscuousPkt::frame_type`
|
||||
pub fn matches(self, frame_type: core::ffi::c_uint) -> bool {
|
||||
self.as_frame_type() == frame_type
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue