From 2379a7eefc69d2342b5041b7d35ee371864d4a71 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Fri, 3 Jul 2026 13:38:54 +0200 Subject: [PATCH] Enable V2X transmission, add optional stand-alone CAM transmission --- Cargo.lock | 22 +++-- Cargo.toml | 5 +- Readme.md | 7 ++ feat-permut.sh | 2 + src/applogic/cam_tx.rs | 197 +++++++++++++++++++++++++++++++++++++++++ src/applogic/mod.rs | 2 + src/main.rs | 68 +++++++++++++- src/v2x_tx/gn.rs | 94 ++++++++++++++++++++ src/v2x_tx/mod.rs | 160 +++++++++++++++++++++++++++++++++ 9 files changed, 543 insertions(+), 14 deletions(-) create mode 100644 src/applogic/cam_tx.rs create mode 100644 src/v2x_tx/gn.rs create mode 100644 src/v2x_tx/mod.rs diff --git a/Cargo.lock b/Cargo.lock index 359840b..3fba778 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -73,6 +73,12 @@ version = "1.0.102" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7f202df86484c868dbad7eaa557ef785d5c66295e41b460ef922eca0723b842c" +[[package]] +name = "arbitrary-int" +version = "2.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "993a810118f8f37e9c4411c86f1c4c940a09a7ab34b7bf2d88d06f50c553fab7" + [[package]] name = "arrayvec" version = "0.7.6" @@ -186,9 +192,9 @@ checksum = "1e748733b7cbc798e1434b6ac524f0c1ff2ab456fe201501e6497c8417a4fc33" [[package]] name = "c-its-parser" -version = "2.3.0" +version = "2.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f9bd739e1f76c017fc9fc6c21b9ff2aee4536bef357075d454e387e94a4d6287" +checksum = "cd4db66f495280ec88eef8712dd9ab7d1aacc590f62485e3e03a89b3ce4f8b14" dependencies = [ "chrono", "etherparse", @@ -1239,9 +1245,9 @@ dependencies = [ [[package]] name = "etherparse" -version = "0.20.1" +version = "0.21.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3ac016aaf11dfe643edcd088a166234bfcb72e7f06691abfcf21e9af524037f4" +checksum = "17304d06addb3283cdc4bd528e42dd95e73c8ee2d6492ffce415e93660885449" dependencies = [ "arrayvec", ] @@ -1371,13 +1377,11 @@ dependencies = [ [[package]] name = "geonetworking" -version = "0.3.0" +version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6f6d36111f5057d3be7eb4e312a07edac4737fc791e4b0a73d812c6f710161e7" +checksum = "6da6fde770112587d5f5f7af439546e35bd35aa77a9e3e47e9968056783fe50d" dependencies = [ - "bitvec", - "bitvec-nom2", - "bytes", + "arbitrary-int", "nom", "num", "num-traits", diff --git a/Cargo.toml b/Cargo.toml index f322c5b..834f164 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -37,6 +37,8 @@ std = ["c-its-parser/std"] gnss = ["dep:embassy_gps"] # echo CAM data on serial cam = ["c-its-parser/cam"] +# send CAM autonomously +cam_tx = ["c-its-parser/cam", "gnss"] # echo DENM events on serial and BLE denm = ["c-its-parser/denm"] # evaluate SPAT information @@ -85,7 +87,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.3.0", default-features = false, features = [ +# needs newer GN with removed bitvec dependency +c-its-parser = { version = "2.4.0", default-features = false, features = [ "time", "geo", "libm", diff --git a/Readme.md b/Readme.md index 8659877..281f995 100644 --- a/Readme.md +++ b/Readme.md @@ -28,6 +28,10 @@ Shall "publish" the most relevant information of a DENMs via BLE ### CAM Just prints a line for each received CAM. +### CAM-TX +Generates and transmits a CAM when a GNSS update was received. +Depends on the `gnss` feature. + ## Usage @@ -60,9 +64,12 @@ Additional features are available through feature flags: - `spat_debug`: Enable additional debug information for the SPAT/ GLOSA use case - `denm`: Enable DENMs - `cam`: Enable CAMs +- `cam_tx`: Enable CAM generation and transmission Officially supported feature combinations are: - `esp,spat,gnss,screen` (default features) +- `esp,spat,gnss,screen,cam_tx` +- `esp,spat,gnss,screen,cam_tx,cam,denm` - To be implemented: No GNSS and screen when BLE/ UART interface is implemented Connect your ESP32-C5 and run: diff --git a/feat-permut.sh b/feat-permut.sh index cd20431..a40a8ca 100755 --- a/feat-permut.sh +++ b/feat-permut.sh @@ -10,9 +10,11 @@ extended=${2:-false} features_base=( #cam,denm,uart spat,gnss,screen + spat,gnss,screen,cam_tx ) features_more=( spat,gnss,screen,cam,denm + spat,gnss,screen,cam_tx,cam,denm ) features_test=( spat diff --git a/src/applogic/cam_tx.rs b/src/applogic/cam_tx.rs new file mode 100644 index 0000000..7de554e --- /dev/null +++ b/src/applogic/cam_tx.rs @@ -0,0 +1,197 @@ +//! C-ITS CAM Transmission + +use alloc::vec::Vec; + +use c_its_parser::gn as geonetworking; +use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions; +use c_its_parser::standards::cdd_1_3_1_1::its_container; +use c_its_parser::standards::extensions; + +#[derive(Debug)] +pub struct CamState { + station_id: u32, + station_type: its_container::StationType, + + vehicle_length_dm: u16, // vehicle length is in 10cm steps + vehicle_width_dm: u8, // vehicle width is in 10cm steps + + time: chrono::DateTime, + pos_state: super::PositionState, +} + +impl CamState { + pub fn new( + station_id: u32, + station_type: c_its_parser::standards::extensions::ItsStationType, + vehicle_length_dm: u16, + vehicle_width_dm: u8, + ) -> Self { + Self { + station_id, + station_type: station_type.into(), + vehicle_length_dm, + vehicle_width_dm, + time: chrono::DateTime::::default(), + pos_state: super::PositionState::default(), + } + } + + pub fn update(&mut self, time: chrono::DateTime, pos: super::PositionState) { + self.time = time; + self.pos_state = pos; + + // TODO: build CAM trace + } + + #[allow(clippy::too_many_lines)] + fn make_cam(&self) -> Result { + let header = its_container::ItsPduHeader::new( + 2, + extensions::ItsMessageId::Cam.as_u8(), + its_container::StationID(self.station_id), + ); + + let generation_delta_time = { + let its_timestamp = c_its_parser::time_utils::TimestampIts::from(self.time); + + #[allow(clippy::cast_possible_truncation)] + let ts_mod = (its_timestamp.0 % 65_536) as u16; + + cam_pdu_descriptions::GenerationDeltaTime(ts_mod) + }; + + let heading = match self.pos_state.heading_deg { + Some(value) => its_container::Heading { + heading_value: its_container::HeadingValue::from_deg(value) + .map_err(|err| alloc::format!("Failed to make HeadingValue: {err}"))?, + heading_confidence: its_container::HeadingConfidence(127), // unavailable for now + }, + None => its_container::Heading { + heading_value: its_container::HeadingValue(0), + heading_confidence: its_container::HeadingConfidence(127), // unavailable for now + }, + }; + let speed = match self.pos_state.speed_mps { + Some(value) => its_container::Speed { + speed_value: its_container::SpeedValue::from_mps(value) + .map_err(|err| alloc::format!("Failed to make SpeedValue: {err}"))?, + speed_confidence: its_container::SpeedConfidence(127), // unavailable for now + }, + None => its_container::Speed { + speed_value: its_container::SpeedValue(0), + speed_confidence: its_container::SpeedConfidence(127), // unavailable for now + }, + }; + let reference_position = its_container::ReferencePosition { + latitude: its_container::Latitude::from_deg(self.pos_state.position.y()), + longitude: its_container::Longitude::from_deg(self.pos_state.position.x()), + position_confidence_ellipse: its_container::PosConfidenceEllipse { + semi_major_confidence: its_container::SemiAxisLength(4095), // unavailable for now + semi_minor_confidence: its_container::SemiAxisLength(4095), // unavailable for now + semi_major_orientation: its_container::HeadingValue::unavailable(), + }, + altitude: its_container::Altitude { + altitude_value: its_container::AltitudeValue(800_001), // unavailable for now + altitude_confidence: its_container::AltitudeConfidence::unavailable, + }, + }; + + let basic_container = cam_pdu_descriptions::BasicContainer::new( + self.station_type.clone(), + reference_position, + ); + let high_frequency_container = + cam_pdu_descriptions::HighFrequencyContainer::basicVehicleContainerHighFrequency( + cam_pdu_descriptions::BasicVehicleContainerHighFrequency { + heading, + speed, + drive_direction: its_container::DriveDirection::unavailable, + vehicle_length: its_container::VehicleLength { + vehicle_length_value: its_container::VehicleLengthValue( + self.vehicle_length_dm, + ), + vehicle_length_confidence_indication: + its_container::VehicleLengthConfidenceIndication::unavailable, + }, + vehicle_width: its_container::VehicleWidth(self.vehicle_width_dm), + longitudinal_acceleration: its_container::LongitudinalAcceleration { + longitudinal_acceleration_value: + its_container::LongitudinalAccelerationValue::unavailable(), + longitudinal_acceleration_confidence: its_container::AccelerationConfidence( + 102, // unavailable + ), + }, + curvature: its_container::Curvature { + curvature_value: its_container::CurvatureValue::unavailable(), + curvature_confidence: its_container::CurvatureConfidence::unavailable, + }, + curvature_calculation_mode: + its_container::CurvatureCalculationMode::unavailable, + yaw_rate: its_container::YawRate { + yaw_rate_value: its_container::YawRateValue::unavailable(), + yaw_rate_confidence: its_container::YawRateConfidence::unavailable, + }, + acceleration_control: None, + lane_position: None, + steering_wheel_angle: None, + lateral_acceleration: None, + vertical_acceleration: None, + performance_class: None, + cen_dsrc_tolling_zone: None, + }, + ); + let low_frequency_container = + cam_pdu_descriptions::LowFrequencyContainer::basicVehicleContainerLowFrequency( + cam_pdu_descriptions::BasicVehicleContainerLowFrequency { + vehicle_role: its_container::VehicleRole::default, + exterior_lights: its_container::ExteriorLights::default(), + path_history: its_container::PathHistory(alloc::vec![]), // TODO: fill trace + }, + ); + + let cam_parameters = cam_pdu_descriptions::CamParameters::new( + basic_container, + high_frequency_container, + Some(low_frequency_container), + None, + ); + + Ok(cam_pdu_descriptions::CAM::new( + header, + cam_pdu_descriptions::CoopAwareness::new(generation_delta_time, cam_parameters), + )) + } +} + +impl crate::v2x_tx::GnItsPayload for CamState { + fn make_eh( + &self, + address: [u8; 6], + own_position: &crate::applogic::PositionState, + time: chrono::DateTime, + _seq_no: &mut u16, + ) -> ( + geonetworking::en302636_4_1::ExtendedHeader, + Option, + u8, + ) { + let station_type = geonetworking::en302636_4_1::StationType::try_from(self.station_type.0) + .unwrap_or_default(); + + // CAM always uses SHB, hop-limit 1 + ( + crate::v2x_tx::gn::make_shb_eh(address, station_type, own_position, time), + None, + 1, + ) + } + + fn make_payload( + &self, + ) -> Result<(Vec, c_its_parser::standards::extensions::ItsMessageId), alloc::string::String> + { + let msg = self.make_cam()?; + let uper = msg.encode_to_uper()?; + Ok((uper, c_its_parser::standards::extensions::ItsMessageId::Cam)) + } +} diff --git a/src/applogic/mod.rs b/src/applogic/mod.rs index 98d100f..d1d082e 100644 --- a/src/applogic/mod.rs +++ b/src/applogic/mod.rs @@ -18,6 +18,8 @@ pub mod v2x; #[cfg(feature = "cam")] pub mod cam; +#[cfg(feature = "cam_tx")] +pub mod cam_tx; #[cfg(feature = "denm")] pub mod denm; #[cfg(feature = "spat")] diff --git a/src/main.rs b/src/main.rs index bddb837..daac5b8 100644 --- a/src/main.rs +++ b/src/main.rs @@ -14,6 +14,8 @@ use core::cell::RefCell; use core::cell::UnsafeCell; use c_its_parser::gn as geonetworking; +#[cfg(feature = "cam_tx")] +use c_its_parser::standards::extensions::ItsStationType; use critical_section::Mutex; use crossbeam_queue::ArrayQueue; use embassy_executor::Spawner; @@ -37,10 +39,20 @@ mod io; mod radio; #[cfg(feature = "screen")] mod screen; +#[cfg(feature = "cam_tx")] +mod v2x_tx; const WIFI_CHANNEL: radio::Channel = 180; #[cfg(feature = "spat")] const SPAT_RATE_LIMIT: u8 = 5; // keep every n-th message +#[cfg(feature = "cam_tx")] +const GN_IS_MOBILE: bool = true; +#[cfg(feature = "cam_tx")] +const OWN_STATION_TYPE: ItsStationType = ItsStationType::Cyclist; +#[cfg(feature = "cam_tx")] +const OWN_VEHICLE_WIDTH: u8 = 5; // in 10cm steps! +#[cfg(feature = "cam_tx")] +const OWN_VEHICLE_LENGTH: u16 = 20; // in 10cm steps! // This creates a default app-descriptor required by the esp-idf bootloader. // For more information see: @@ -119,7 +131,7 @@ async fn main(spawner: Spawner) -> ! { // Setup WiFi // sadly, it doesn't work any more when we move `esp_radio::wifi::new` to `setup_wifi_sniffer` - let (mut wifi_controller, mut interfaces) = esp_radio::wifi::new( + let (mut wifi_controller, interfaces) = esp_radio::wifi::new( peripherals.WIFI, esp_radio::wifi::ControllerConfig::default(), ) @@ -131,7 +143,8 @@ async fn main(spawner: Spawner) -> ! { critical_section::with(|cs| { WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2))); }); - radio::setup_wifi_sniffer(WIFI_CHANNEL, &mut interfaces.sniffer, handle_frame) + let mut wlan_iface = interfaces.sniffer; + radio::setup_wifi_sniffer(WIFI_CHANNEL, &mut wlan_iface, handle_frame) .expect("Fatal error initializing 802.11p sniffer"); info!("WiFi promiscuous mode on channel {WIFI_CHANNEL} running"); @@ -159,6 +172,23 @@ async fn main(spawner: Spawner) -> ! { info!("GNSS task started, waiting for GNSS fix..."); let mut state = applogic::State::new(); + #[cfg(feature = "cam_tx")] + let (mut seq_no, mut cam, mac_bytes) = { + let station_id = v2x_tx::make_station_id(mac); + info!("V2X Station ID: {station_id}"); + + // unwrap is fine since [`esp_hal::efuse::MacAddress`] is just an `[u8; 6]` + let mac_bytes = mac.as_bytes().try_into().unwrap(); + + let cam = applogic::cam_tx::CamState::new( + station_id, + OWN_STATION_TYPE, + OWN_VEHICLE_LENGTH, + OWN_VEHICLE_WIDTH, + ); + + (0, cam, mac_bytes) + }; loop { embassy_time::Timer::after(embassy_time::Duration::from_millis(1)).await; @@ -171,6 +201,33 @@ async fn main(spawner: Spawner) -> ! { if let Some(fix) = gnss_update_ref.replace(None) { state.update_with_gpsfix(&fix); new_position = true; + + #[cfg(feature = "cam_tx")] + { + use v2x_tx::GnItsPayload; + let time = state.time().and_utc(); + + cam.update(time, fix.clone().into()); + + match cam.encode_packet( + mac_bytes, + &fix.into(), + time, + GN_IS_MOBILE, + &mut seq_no, + ) { + Ok(data) => { + if let Err(err) = radio::send_80211_bcast_frame( + &mut wlan_iface, + mac.as_bytes(), + &data, + ) { + warn!("Failed to send 802.11 frame: {err}"); + } + } + Err(err) => warn!("Failed to create CAM: {err}"), + } + } } }); @@ -236,12 +293,15 @@ async fn main(spawner: Spawner) -> ! { } } - #[cfg(feature = "cam")] + #[cfg(any(feature = "cam", feature = "cam_tx"))] ItsMessage::Cam { geonetworking: _, transport: _, etsi, - } => applogic::cam::handle_cam(&etsi), + } => { + #[cfg(feature = "cam")] + applogic::cam::handle_cam(&etsi); + } #[cfg(feature = "denm")] ItsMessage::DenmV2 { diff --git a/src/v2x_tx/gn.rs b/src/v2x_tx/gn.rs new file mode 100644 index 0000000..828c74d --- /dev/null +++ b/src/v2x_tx/gn.rs @@ -0,0 +1,94 @@ +//! Geonetworking utilities + +use c_its_parser::gn as geonetworking; + +#[allow(unused)] +pub fn make_shb_eh( + address: [u8; 6], + station_type: geonetworking::en302636_4_1::StationType, + own_position: &crate::applogic::PositionState, + time: chrono::DateTime, +) -> geonetworking::en302636_4_1::ExtendedHeader { + let source_position_vector = make_lpv(address, station_type, own_position, time); + + geonetworking::en302636_4_1::ExtendedHeader::SHB( + geonetworking::en302636_4_1::SingleHopBroadcast { + source_position_vector, + media_dependent_data: [0; 4], + }, + ) +} + +#[allow(unused)] +pub fn make_tsb_eh( + address: [u8; 6], + station_type: geonetworking::en302636_4_1::StationType, + own_position: &crate::applogic::PositionState, + time: chrono::DateTime, + sequence_number: u16, +) -> geonetworking::en302636_4_1::ExtendedHeader { + let source_position_vector = make_lpv(address, station_type, own_position, time); + + geonetworking::en302636_4_1::ExtendedHeader::TSB( + geonetworking::en302636_4_1::TopologicallyScopedBroadcast::new( + sequence_number, + source_position_vector, + ), + ) +} + +#[allow(unused)] +pub fn make_gbc_eh( + address: [u8; 6], + station_type: geonetworking::en302636_4_1::StationType, + own_position: &crate::applogic::PositionState, + time: chrono::DateTime, + target_pos: geo_types::Point, + radius_m: u16, + sequence_number: u16, +) -> geonetworking::en302636_4_1::ExtendedHeader { + let source_position_vector = make_lpv(address, station_type, own_position, time); + #[allow(clippy::cast_possible_truncation)] + let latitude_deg = target_pos.x() as f32; + #[allow(clippy::cast_possible_truncation)] + let longitude_deg = target_pos.y() as f32; + + geonetworking::en302636_4_1::ExtendedHeader::GBC( + geonetworking::en302636_4_1::GeoBroadcast::try_from_values( + sequence_number, + source_position_vector, + latitude_deg, + longitude_deg, + radius_m, + 0, + 0, + ) + .expect("Failed to create circular GBC"), + ) +} + +fn make_lpv( + address: [u8; 6], + station_type: geonetworking::en302636_4_1::StationType, + own_position: &crate::applogic::PositionState, + time: chrono::DateTime, +) -> geonetworking::en302636_4_1::LongPositionVector { + let timestamp_its = c_its_parser::time_utils::TimestampIts::from(time); + #[allow(clippy::cast_possible_truncation)] + let latitude_deg = own_position.position.y() as f32; + #[allow(clippy::cast_possible_truncation)] + let longitude_deg = own_position.position.x() as f32; + let speed_mps = own_position.speed_mps.unwrap_or_default(); + let heading_deg = own_position.heading_deg.unwrap_or_default(); + + geonetworking::en302636_4_1::LongPositionVector::try_from_values( + geonetworking::en302636_4_1::Address::new(false, station_type, address), + timestamp_its.0, + latitude_deg, + longitude_deg, + true, + speed_mps, + heading_deg, + ) + .expect("Failed to create LongPositionVector") +} diff --git a/src/v2x_tx/mod.rs b/src/v2x_tx/mod.rs new file mode 100644 index 0000000..e9b1a28 --- /dev/null +++ b/src/v2x_tx/mod.rs @@ -0,0 +1,160 @@ +//! V2X Transmission + +use alloc::vec::Vec; + +use c_its_parser::gn::{self as geonetworking, Encode}; + +pub mod gn; + +#[cfg(feature = "esp")] +/// Creates a station ID from the WLAN MAC address +/// +/// Will use `0xC173` as static prefix and uses last 16 bit from MAC address for the remainder. +pub fn make_station_id(mac: esp_hal::efuse::MacAddress) -> u32 { + // unwrap is fine here since we can be sure that the MacAddress is 6 byte + // and a slice of 2 bytes can be converted to [u8; 2] + let mac_last_16bit = u16::from_be_bytes(mac.as_bytes().get(4..6).unwrap().try_into().unwrap()); + + 0xC173_0000 | u32::from(mac_last_16bit) +} + +pub trait GnItsPayload { + // common GN parameters + const LIFETIME_MILLIS: u32 = 60_000; // itsGnDefaultPacketLifetime is 60 seconds according to ETSI EN 302 636-4-1 V1.4.1 + + // --------------------------------------------- + // methods to build a GN packet with ITS payload + // --------------------------------------------- + + /// Builds the [`geonetworking::en302636_4_1::ExtendedHeader`] and additional information for the ITS message + /// + /// Output: + /// - Extended header + /// - Area type (when using GAC or GBC) + /// - hop limit + /// + /// Note: Make sure to increase the sequence number on multi-hop packets. + fn make_eh( + &self, + address: [u8; 6], + own_position: &crate::applogic::PositionState, + time: chrono::DateTime, + seq_no: &mut u16, + ) -> ( + geonetworking::en302636_4_1::ExtendedHeader, + Option, + u8, + ); + + /// Creates the ITS payload (without BTP header) + fn make_payload( + &self, + ) -> Result<(Vec, c_its_parser::standards::extensions::ItsMessageId), alloc::string::String>; + + // --------------------------------------------- + // common methods (with implementation) + // --------------------------------------------- + + /// Encodes the GN packet + fn encode_packet( + &self, + address: [u8; 6], + own_position: &crate::applogic::PositionState, + time: chrono::DateTime, + is_mobile: bool, + seq_no: &mut u16, + ) -> Result, alloc::string::String> { + // Build payload (including BTP header) + let (mut its_payload, msg_type) = self.make_payload()?; + let mut payload = make_btp_b(msg_type).encode()?; + + payload.append(&mut its_payload); + + #[allow(clippy::cast_possible_truncation)] + let payload_length = (payload.len()) as u16; + + // get extended header (depending on message type and sometimes content as well) + let (eh, area_type, maximum_hop_limit) = self.make_eh(address, own_position, time, seq_no); + + // Build GN headers + let traffic_class = geonetworking::en302636_4_1::TrafficClass::try_new( + false, + false, + make_traffic_class(msg_type), + ) + .map_err(|err| alloc::format!("Failed to create TrafficClass: {err:?}"))?; + let common = geonetworking::en302636_4_1::CommonHeader::from_values( + geonetworking::en302636_4_1::NextAfterCommon::BTPB, + geonetworking::en302636_4_1::HeaderType::new_from(&eh, area_type)?, + traffic_class, + is_mobile, + payload_length, + maximum_hop_limit, + ); + + let remaining_hop_limit = maximum_hop_limit; + let basic = geonetworking::en302636_4_1::BasicHeader::try_new( + 1, + geonetworking::en302636_4_1::NextAfterBasic::CommonHeader, + geonetworking::en302636_4_1::Lifetime::from_milliseconds(Self::LIFETIME_MILLIS), + remaining_hop_limit, + ) + .map_err(|err| alloc::format!("Failed to create BasicHeader: {err}"))?; + + // Assemble and encode packet + let packet = geonetworking::Packet::Unsecured { + basic, + common, + extended: Some(eh), + payload: &payload, + }; + + packet + .encode_to_vec() + .map_err(|err| alloc::format!("Failed to encode GN packet: {err:?}")) + } +} + +/// Creates a BTP-B header based on the ITS message ID (message type) +/// +/// Port numbers and destination port info according to ETSI TS 103 248 +fn make_btp_b( + msg_type: c_its_parser::standards::extensions::ItsMessageId, +) -> c_its_parser::transport::TransportHeader { + let (destination_port, destination_port_info) = match msg_type { + c_its_parser::standards::extensions::ItsMessageId::Denm => (2002, 0), + c_its_parser::standards::extensions::ItsMessageId::Cam => (2001, 0), + c_its_parser::standards::extensions::ItsMessageId::Mapem => (2003, 0), + c_its_parser::standards::extensions::ItsMessageId::Spatem => (2004, 0), + c_its_parser::standards::extensions::ItsMessageId::Saem => (2005, 0), + c_its_parser::standards::extensions::ItsMessageId::Ivim => (2006, 0), + c_its_parser::standards::extensions::ItsMessageId::Srem => (2007, 0), + c_its_parser::standards::extensions::ItsMessageId::Ssem => (2008, 0), + c_its_parser::standards::extensions::ItsMessageId::Cpm => (2009, 0), + c_its_parser::standards::extensions::ItsMessageId::Poi => (2010, 0), + c_its_parser::standards::extensions::ItsMessageId::EvRsr => (2012, 0), + c_its_parser::standards::extensions::ItsMessageId::Rtcmem => (2013, 0), + _ => (0, 0), // no information found for the subsequent types + }; + + c_its_parser::transport::TransportHeader::BtpB(c_its_parser::transport::BasicTransportBHeader { + destination_port, + destination_port_info, + }) +} + +/// Creates a traffic class ID based on the ITS message ID (message type) +fn make_traffic_class(msg_type: c_its_parser::standards::extensions::ItsMessageId) -> u8 { + #[allow(clippy::match_same_arms)] + match msg_type { + c_its_parser::standards::extensions::ItsMessageId::Denm => 1, + c_its_parser::standards::extensions::ItsMessageId::Cam => 2, + c_its_parser::standards::extensions::ItsMessageId::Mapem => 3, // from C-Roads RS_RSP_063(1) + c_its_parser::standards::extensions::ItsMessageId::Spatem => 3, // from C-Roads RS_RSP_063(1) + c_its_parser::standards::extensions::ItsMessageId::Ivim => 3, // from C-Roads RS_RSP_063(1) + c_its_parser::standards::extensions::ItsMessageId::Srem => 2, // from C-Roads RS_RSP_063(1) + c_its_parser::standards::extensions::ItsMessageId::Ssem => 2, // from C-Roads RS_RSP_063(1) + c_its_parser::standards::extensions::ItsMessageId::Rtcmem => 3, // CSP_MaxPrio=252 -> 3 + _ => 3, // fall-back to lowest priority + } +}