diff --git a/Cargo.lock b/Cargo.lock index b3d5c4c..98a4c6a 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -192,9 +192,9 @@ checksum = "1e748733b7cbc798e1434b6ac524f0c1ff2ab456fe201501e6497c8417a4fc33" [[package]] name = "c-its-parser" -version = "2.4.0" +version = "2.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cd4db66f495280ec88eef8712dd9ab7d1aacc590f62485e3e03a89b3ce4f8b14" +checksum = "7e94d9990b20498f621f164f23ae198c4365115eb4191ae106352ec74d4a4110" dependencies = [ "chrono", "etherparse", @@ -1171,6 +1171,7 @@ dependencies = [ "esp-println", "esp-radio", "esp-rtos", + "esp32-cits-core", "geo-types", "log", "mipidsi", @@ -1181,6 +1182,18 @@ dependencies = [ "ublox", ] +[[package]] +name = "esp32-cits-core" +version = "0.1.0" +source = "git+https://git.hamburg.ccc.de/jtbx/esp32-cits-core?branch=main#a6d0ea6ca2da130fde2b84060a34ef45e772a459" +dependencies = [ + "c-its-parser", + "chrono", + "esp-hal", + "esp-radio", + "geo-types", +] + [[package]] name = "esp32c2" version = "0.29.2" diff --git a/Cargo.toml b/Cargo.toml index 55f42c5..9b3cbd9 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -30,8 +30,10 @@ esp = [ "dep:esp-backtrace", "dep:esp-println", "dep:esp-radio", + "esp32-cits-core/esp32c5", + "esp32-cits-core/log-04", ] -std = ["c-its-parser/std"] +std = ["c-its-parser/std", "esp32-cits-core/std"] # enable u-blox GNSS module gnss_ubx = ["_gnss", "_uart", "dep:ublox"] @@ -93,6 +95,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" } +esp32-cits-core = { version = "0.1.0", default-features = false, git = "https://git.hamburg.ccc.de/jtbx/esp32-cits-core", branch = "main" } embedded-hal-bus = { version = "0.3.0", optional = true } embedded-graphics = { version = "0.8.2", optional = true } # needs newer GN with removed bitvec dependency diff --git a/src/applogic/cam_tx.rs b/src/applogic/cam_tx.rs index e15de27..850ae40 100644 --- a/src/applogic/cam_tx.rs +++ b/src/applogic/cam_tx.rs @@ -11,6 +11,45 @@ const PH_MAX_LENGHT_M: f32 = 500.; const PH_MAX_ITEMS: usize = 23; const PH_MIN_DIST_M: f32 = 20.; +/// Stand-alone CAM generator +/// +/// Generates CAM messages from time and [`PosVel`] updates +/// +/// # Send a message +/// ``` +/// use esp32_cits_core::tx::GnItsPayload as _; +/// +/// let (mut seq_no, mac_bytes) = { +/// // unwrap is fine since [`esp_hal::efuse::MacAddress`] is just an `[u8; 6]` +/// let mac_bytes = mac.as_bytes().try_into().unwrap(); +/// +/// (0, mac_bytes) +/// }; +/// +/// let mut cam = { +/// let station_id = make_station_id(mac); +/// info!("V2X Station ID: {station_id}"); +/// +/// applogic::cam_tx::CamState::new( +/// station_id, +/// OWN_STATION_TYPE, +/// OWN_VEHICLE_LENGTH, +/// OWN_VEHICLE_WIDTH, +/// ) +/// }; +/// +/// cam.update(time, fix.clone().into()); +/// match cam.encode_packet(mac_bytes, pos_vel, time, GN_IS_MOBILE, &mut seq_no) { +/// Ok(data) => { +/// if let Err(err) = +/// radio::send_80211_bcast_frame(&mut wlan_iface, &mac_bytes.clone(), &data) +/// { +/// warn!("Failed to send 802.11 frame: {err}"); +/// } +/// } +/// Err(err) => warn!("Failed to create CAM: {err}"), +/// } +/// ``` #[derive(Debug)] pub struct CamState { station_id: u32, @@ -20,7 +59,7 @@ pub struct CamState { vehicle_width_dm: u8, // vehicle width is in 10cm steps time: chrono::DateTime, - pos_state: super::PositionState, + pos_state: esp32_cits_core::PosVel, /// database for path history, oldest item at the back of the vector path_history: Vec, @@ -35,7 +74,7 @@ struct PathPoint { } impl PathPoint { - fn new_from_pos(pos: &super::PositionState, time: chrono::DateTime) -> Self { + fn new_from_pos(pos: &esp32_cits_core::PosVel, time: chrono::DateTime) -> Self { Self { pos: pos.position, time, @@ -44,7 +83,7 @@ impl PathPoint { } fn new_from_pos_with_dist( - pos: &super::PositionState, + pos: &esp32_cits_core::PosVel, time: chrono::DateTime, dist_m: f32, ) -> Self { @@ -69,12 +108,12 @@ impl CamState { vehicle_length_dm, vehicle_width_dm, time: chrono::DateTime::::default(), - pos_state: super::PositionState::default(), + pos_state: esp32_cits_core::PosVel::default(), path_history: Vec::with_capacity(PH_MAX_ITEMS + 1), } } - pub fn update(&mut self, time: chrono::DateTime, pos: super::PositionState) { + pub fn update(&mut self, time: chrono::DateTime, pos: esp32_cits_core::PosVel) { self.time = time; self.pos_state = pos; @@ -86,7 +125,7 @@ impl CamState { fn update_ph( path_history: &mut Vec, time: chrono::DateTime, - pos: &super::PositionState, + pos: &esp32_cits_core::PosVel, ) { // Update path history if path_history.is_empty() { @@ -293,11 +332,11 @@ impl CamState { } } -impl crate::v2x_tx::GnItsPayload for CamState { +impl esp32_cits_core::tx::GnItsPayload for CamState { fn make_eh( &self, address: [u8; 6], - own_position: &crate::applogic::PositionState, + own_position: &esp32_cits_core::PosVel, time: chrono::DateTime, _seq_no: &mut u16, ) -> Result< @@ -313,7 +352,7 @@ impl crate::v2x_tx::GnItsPayload for CamState { // CAM always uses SHB, hop-limit 1 Ok(( - crate::v2x_tx::gn::make_shb_eh(address, station_type, own_position, time), + esp32_cits_core::tx::gn::make_shb_eh(address, station_type, own_position, time)?, None, 1, )) @@ -336,8 +375,8 @@ mod tests { use super::*; use crate::init_test_env_logger; - fn posstate_from_pos(position: geo_types::Point) -> crate::applogic::PositionState { - crate::applogic::PositionState { + fn posstate_from_pos(position: geo_types::Point) -> esp32_cits_core::PosVel { + esp32_cits_core::PosVel { position, heading_deg: None, speed_mps: None, diff --git a/src/applogic/mod.rs b/src/applogic/mod.rs index 7847010..70cc086 100644 --- a/src/applogic/mod.rs +++ b/src/applogic/mod.rs @@ -49,7 +49,7 @@ pub struct State { time: chrono::NaiveDateTime, #[allow(unused)] - pos: PositionState, + pos: esp32_cits_core::PosVel, #[cfg(feature = "spat")] last_prune: chrono::NaiveDateTime, @@ -130,15 +130,7 @@ pub struct GnssFix { pub speed_mps: Option, } -#[allow(unused)] -#[derive(Debug, Default, Clone, PartialEq)] -pub struct PositionState { - pub position: geo_types::Point, - pub heading_deg: Option, - pub speed_mps: Option, -} - -impl From<&GnssFix> for PositionState { +impl From<&GnssFix> for esp32_cits_core::PosVel { fn from(value: &GnssFix) -> Self { let position = geo_types::Point::new(value.longitude_deg, value.latitude_deg); @@ -149,7 +141,7 @@ impl From<&GnssFix> for PositionState { } } } -impl From for PositionState { +impl From for esp32_cits_core::PosVel { fn from(value: GnssFix) -> Self { (&value).into() } @@ -212,7 +204,7 @@ impl State { Self { initialized: false, time: chrono::NaiveDateTime::default(), - pos: PositionState::default(), + pos: esp32_cits_core::PosVel::default(), #[cfg(feature = "spat")] last_prune: chrono::NaiveDateTime::default(), #[cfg(feature = "spat")] @@ -641,7 +633,7 @@ impl State { } #[allow(unused)] - pub fn pos(&self) -> &PositionState { + pub fn pos(&self) -> &esp32_cits_core::PosVel { &self.pos } } diff --git a/src/io.rs b/src/io.rs index 4642253..44997ee 100644 --- a/src/io.rs +++ b/src/io.rs @@ -169,7 +169,7 @@ impl From for crate::applogic::GnssFix { } } -impl From for crate::applogic::PositionState { +impl From for esp32_cits_core::PosVel { fn from(value: msg::PositionState) -> Self { let position = geo_types::Point::new( value.position.longitude_deg.into(), @@ -194,7 +194,7 @@ impl From for crate::applogic::PositionState { } } -impl From for crate::applogic::PositionState { +impl From for esp32_cits_core::PosVel { fn from(value: msg::ItsPosition) -> Self { let latitude_deg = f64::from(value.latitude) / 10_000_000.; let longitude_deg = f64::from(value.longitude) / 10_000_000.; @@ -208,11 +208,11 @@ impl From for crate::applogic::PositionState { } } -impl crate::v2x_tx::GnItsPayload for msg::RawMsgTx { +impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx { fn make_eh( &self, address: [u8; 6], - own_position: &crate::applogic::PositionState, + own_position: &esp32_cits_core::PosVel, time: chrono::DateTime, seq_no: &mut u16, ) -> Result< @@ -275,19 +275,19 @@ impl crate::v2x_tx::GnItsPayload for msg::RawMsgTx { *seq_no += 1; ( - crate::v2x_tx::gn::make_tsb_eh( + esp32_cits_core::tx::gn::make_tsb_eh( address, station_type, own_position, time, *seq_no, - ), + )?, None, hop_limit, ) } msg::GnTransport::Shb => ( - crate::v2x_tx::gn::make_shb_eh(address, station_type, own_position, time), + esp32_cits_core::tx::gn::make_shb_eh(address, station_type, own_position, time)?, None, 1, ), @@ -308,7 +308,7 @@ impl msg::RawMsgTx { fn make_geobcast( address: [u8; 6], station_type: geonetworking::en302636_4_1::StationType, - own_position: &crate::applogic::PositionState, + own_position: &esp32_cits_core::PosVel, time: chrono::DateTime, gn_area: msg::GnArea, sequence_number: u16, @@ -320,7 +320,7 @@ impl msg::RawMsgTx { alloc::string::String, > { let source_position_vector = - crate::v2x_tx::gn::make_lpv(address, station_type, own_position, time); + esp32_cits_core::tx::gn::make_lpv(address, station_type, own_position, time)?; let (atype, dist_a, dist_b, angle) = match gn_area.area() { msg::GnAreaShape::EtsiAreashapeCircle => ( diff --git a/src/main.rs b/src/main.rs index 5320951..67e181c 100644 --- a/src/main.rs +++ b/src/main.rs @@ -38,7 +38,7 @@ use esp_hal::clock::CpuClock; use esp_hal::timer::timg::TimerGroup; #[cfg(all(target_arch = "riscv32", feature = "spat"))] use esp_println::println; -use esp_radio::wifi; +use esp32_cits_core::radio; use geonetworking::Decode as _; use log::{error, info, warn}; @@ -49,13 +49,10 @@ mod cache; mod geo_alg; #[cfg(feature = "uart")] mod io; -mod radio; #[cfg(feature = "screen")] mod screen; #[cfg(feature = "gnss_ubx")] mod ubx; -#[cfg(any(feature = "cam_tx", feature = "uart"))] -mod v2x_tx; const WIFI_CHANNEL: radio::Channel = 180; const WIFI_TXPOWER: i8 = 10; // dBm, 2..20 @@ -244,7 +241,7 @@ async fn main(spawner: Spawner) -> ! { let mut state = applogic::State::new(); #[cfg(feature = "cam_tx")] let mut cam = { - let station_id = v2x_tx::make_station_id(mac); + let station_id = make_station_id(mac); info!("V2X Station ID: {station_id}"); applogic::cam_tx::CamState::new( @@ -289,7 +286,7 @@ async fn main(spawner: Spawner) -> ! { }); } Some(io::msg::serial_input_msg::Payload::TxMsg(tx_request)) => { - use v2x_tx::GnItsPayload as _; + use esp32_cits_core::tx::GnItsPayload as _; info!( "UART: Got new TX message: Type {} with {} bytes", @@ -380,7 +377,7 @@ async fn main(spawner: Spawner) -> ! { #[cfg(feature = "cam_tx")] { - use v2x_tx::GnItsPayload; + use esp32_cits_core::tx::GnItsPayload as _; let time = state.time().and_utc(); cam.update(time, fix.clone().into()); @@ -513,7 +510,7 @@ async fn main(spawner: Spawner) -> ! { reason = "adhering to callback interface" )] #[allow(clippy::too_many_lines)] -fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) { +fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) { #[cfg(feature = "spat")] const PRUNE_INTERVAL: chrono::Duration = chrono::Duration::seconds(60); #[cfg(feature = "spat")] @@ -534,27 +531,18 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) { .naive_utc() }; - // Ignore frames with errors and non-data frames - if frame.rx_cntl.rx_state != 0 { - warn!("Received frame has RX error: {}", frame.rx_cntl.rx_state); - return; - } - if !radio::PromiscuousPktType::Data.matches(frame.frame_type) { - return; - } - - // 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; + match esp32_cits_core::rx::is_broadcast_frame(&frame) { + Err(err) => { + warn!("{err}"); + return; + } + Ok(false) => { + // drop frame + return; + } + Ok(true) => { + // continue + } } // info!("Received frame with {} bytes", frame.len); @@ -885,3 +873,19 @@ fn send_uart0(data: &[u8]) { } }); } + +#[cfg(all(feature = "esp", feature = "cam_tx"))] +/// 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. +/// +/// # Panics +/// Won't panic in practive b/c of pre-conditions +#[must_use] +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) +} diff --git a/src/radio.rs b/src/radio.rs deleted file mode 100644 index 46cd72a..0000000 --- a/src/radio.rs +++ /dev/null @@ -1,288 +0,0 @@ -//! IEEE 802.11p C-ITS Capture with ESP32-C5 -#![allow(unused)] - -// ----------------------------- -// missing interface definitions -// ----------------------------- - -pub type Channel = core::ffi::c_int; - -unsafe extern "C" { - /// Changes the 802.11 PHY channel - /// - /// C function signature: `void phy_change_channel(int,int,int,int)` - /// - /// Options: - /// - `primary`: Channel frequency in MHz - /// - `ignored1`: Ignored (set to 1) - /// - `ignored2`: Ignored (set to 0) - /// - `ht_mode`: Probably HT mode - unsafe fn phy_change_channel( - primary: Channel, - ignored1: core::ffi::c_int, - ignored2: core::ffi::c_int, - ht_mode: core::ffi::c_int, - ) -> core::ffi::c_int; - - /// Enables 802.11p mode on the ESP32-C5 - /// - /// C function signature: `void phy_11p_set(int,int)` - /// - /// Options: - /// - `enable`: Boolean option to enable 802.11p mode - /// - `half_rate`: 5 MHz mode - unsafe fn phy_11p_set(enable: u8, half_rate: u8); - -} - -/// Enum values for `wifi_promiscuous_pkt_type_t` used in `esp_radio::wifi::sniffer::PromiscuousPkt::frame_type` -/// -/// Original value is actually `core::ffi::c_uint`, but that can't be used in `#[repr()]` attributes, -/// so we're using `u8` as the smallest suitable type here that can be casted to a bigger type. -#[repr(u8)] -#[derive(Debug, Clone, Copy)] -#[allow(dead_code)] -pub(crate) enum PromiscuousPktType { - /// Management frame, indicates 'buf' argument is `wifi_promiscuous_pkt_t` - Mgmt = 0, - /// Control frame, indicates 'buf' argument is `wifi_promiscuous_pkt_t` - Ctrl = 1, - /// Data frame, indicates 'buf' argument is `wifi_promiscuous_pkt_t` - Data = 2, - /// Other type, such as MIMO etc. 'buf' argument is `wifi_promiscuous_pkt_t` but the payload is zero length. - Misc = 3, -} - -impl PromiscuousPktType { - #[allow(dead_code)] - pub fn as_repr(self) -> u8 { - self as u8 - } - - /// Converts to underlying type of `wifi_promiscuous_pkt_type_t` (used by `esp_radio::wifi::sniffer::PromiscuousPkt::frame_type`) - pub fn as_frame_type(self) -> core::ffi::c_uint { - core::ffi::c_uint::from(self.as_repr()) - } - - /// 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 - } -} - -#[cfg(feature = "esp")] -/// Configure WiFi Sniffer to a certain 802.11p channel -pub fn setup_wifi_sniffer( - channel: Channel, - sniffer: &mut esp_radio::wifi::sniffer::Sniffer, - callback: fn(esp_radio::wifi::sniffer::PromiscuousPkt<'_>), -) -> Result<(), alloc::string::String> { - sniffer - .set_promiscuous_mode(true) - .map_err(|err| alloc::format!("Failed to enable promiscuous mode: {err:?}"))?; - - sniffer.set_receive_cb(callback); - - // convert channel number to frequency in MHz - let primary = 5000 + (channel * 5); - - unsafe { - phy_11p_set(1, 0); - match phy_change_channel(primary, 1, 0, 0) { - 0 => Ok(()), - ret => Err(alloc::format!("Failed to change channel: {ret}")), - } - } -} - -#[cfg(feature = "esp")] -/// Sends 802.11p broadcast frame -/// -/// Expects `data` to contain GN header (BTP header and ITS payload) to be a valid C-ITS/ ITS-G5 packet/ frame. -/// -/// # Errors -/// Fails when MAC is not the right size or returns frame send error -pub fn send_80211_bcast_frame( - sniffer: &mut esp_radio::wifi::sniffer::Sniffer, - mac_addr: &[u8], - data: &[u8], -) -> Result<(), alloc::string::String> { - const HEADER_SIZE: usize = 24 + 8; - - let mac = mac_addr - .try_into() - .map_err(|err| alloc::format!("Unexpected MAC address size: {err}"))?; - - // assemble frame - let mut frame_buf = alloc::vec::Vec::with_capacity(HEADER_SIZE + data.len()); - frame_buf.extend_from_slice(&DataFrameHeader::new_80211p(mac).to_binary()); - frame_buf.extend_from_slice(&LlcHeader::new_80211p().to_binary()); - frame_buf.extend_from_slice(data); - - // send - sniffer - .send_raw_frame(true, &frame_buf, true) - .map_err(|err| alloc::format!("Failed to send 802.11p frame: {err:?}")) -} - -/// IEEE 802.11p frame header -#[derive(Debug, Default)] -struct DataFrameHeader { - /// byte 0: subtype in upper nibble, type in lower nibble - /// "normal" data frame: 0x08, QoS data frame: 0x88 - pub type_and_subtype: u8, - /// byte 1: frame contol field, 0x00 for 802.11p - pub fcf_flags: u8, - /// byte 2..3: duration in micros, usually 0x0000 for 802.11p (toDS = 0, fromDS = 0) - pub duration: u16, - /// byte 4..9: receiver/ destination for 802.11p - pub address_1: [u8; 6], - /// byte 10..15: transmitter/ source for 802.11p - pub address_2: [u8; 6], - /// byte 16..21: BSSID for 802.11p - pub address_3: [u8; 6], - /// byte 22..24: upper 12 bits: seq. no, lower 4 bits: fragment no - pub sequence_control: u16, - // Note: address 4 is not used in 802.11p - // pub qos: Option<[u8; 2]>, // QoS frames are supported by radio library - // Note: 802.11p does not use HT mode(s) -} - -impl DataFrameHeader { - const BCAST_MAC: [u8; 6] = [0xFF; 6]; - - /// Create an 802.11p (non-QoS) data frame header with a source MAC address - pub(crate) fn new_80211p(source_mac: &[u8; 6]) -> Self { - Self { - type_and_subtype: 0x08, - address_1: Self::BCAST_MAC, - address_2: *source_mac, - address_3: Self::BCAST_MAC, - ..Default::default() - } - } - - pub(crate) fn to_binary(&self) -> [u8; 24] { - let mut buf = [0u8; 24]; - - buf[0] = self.type_and_subtype; - - buf[1] = self.fcf_flags; - - Self::write_u16(&mut buf, 2, self.duration); - Self::write_mac_addr(&mut buf, 4, &self.address_1); - Self::write_mac_addr(&mut buf, 10, &self.address_2); - Self::write_mac_addr(&mut buf, 16, &self.address_3); - Self::write_u16(&mut buf, 22, self.sequence_control); - - buf - } - - fn write_u16(buf: &mut [u8; 24], offset: usize, data: u16) { - for (idx, byte) in data.to_be_bytes().iter().enumerate() { - buf[idx + offset] = *byte; - } - } - - fn write_mac_addr(buf: &mut [u8; 24], offset: usize, data: &[u8; 6]) { - for (idx, byte) in data.iter().enumerate() { - buf[idx + offset] = *byte; - } - } -} - -/// IEEE 802.11p LLC header -#[derive(Debug, Default)] -struct LlcHeader { - /// byte 0: SNAP (0xaa) in 802.11p - pub dsap: u8, // SNAP in - /// byte 1: SNAP (0xaa) in 802.11p - pub lsap: u8, // SNAP in - /// byte 2: SNAP (0xaa) in 802.11p - pub control_1: u8, // 0x03 - // Note: optional second control byte not used in 802.11p - - // SNAP fields - /// set to 0 since ethertype is used for protocol ID in 802.11p - pub oui: [u8; 3], - /// ethertype GeoNetworking (0x8947) in 802.11p - pub protocol_id: u16, -} - -impl LlcHeader { - pub(crate) fn new_80211p() -> Self { - Self { - dsap: 0xaa, - lsap: 0xaa, - control_1: 0x03, - oui: [0; 3], - protocol_id: 0x8947, - } - } - - pub(crate) fn to_binary(&self) -> [u8; 8] { - let mut buf: [u8; 8] = [ - self.dsap, - self.lsap, - self.control_1, - 0x00, - 0x00, - 0x00, - 0x00, - 0x00, - ]; - - buf[3] = self.oui[0]; - buf[4] = self.oui[1]; - buf[5] = self.oui[2]; - - buf[6] = self.protocol_id.to_be_bytes()[0]; - buf[7] = self.protocol_id.to_be_bytes()[1]; - - buf - } -} - -#[cfg(test)] -mod tests { - - use super::*; - - #[test] - fn data_frame_test() { - // build a typical IEEE 802.11p frame - const OWN_MAC: [u8; 6] = [0x12, 0x34, 0x56, 0x78, 0xab, 0xcd]; - const REF_HEADER: [u8; 24] = [ - 0x08, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x12, 0x34, 0x56, 0x78, - 0xab, 0xcd, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, - ]; - - let frame = DataFrameHeader::new_80211p(&OWN_MAC); - - assert_eq!(0x08, frame.type_and_subtype); // data frame, subtype 0 - assert_eq!(0x0000, frame.fcf_flags); - assert_eq!(0x0000, frame.duration); - assert_eq!([0xFF; 6], frame.address_1); - assert_eq!(OWN_MAC, frame.address_2); - assert_eq!([0xFF; 6], frame.address_3); - assert_eq!(0x0000, frame.sequence_control); - - assert_eq!(REF_HEADER, frame.to_binary()); - } - - #[test] - fn llc_test() { - // build a typical IEEE 802.11p LLC header - const REF_HEADER: [u8; 8] = [0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00, 0x89, 0x47]; - - let llc = LlcHeader::new_80211p(); - - assert_eq!(0xaa, llc.dsap); - assert_eq!(0xaa, llc.lsap); - assert_eq!(0x03, llc.control_1); - assert_eq!([0x00; 3], llc.oui); - assert_eq!(0x8947, llc.protocol_id); - - assert_eq!(REF_HEADER, llc.to_binary()); - } -} diff --git a/src/test.rs b/src/test.rs index 6bb20d2..a7a5bb5 100644 --- a/src/test.rs +++ b/src/test.rs @@ -7,9 +7,7 @@ mod applogic; mod cache; mod geo_alg; mod io; -mod radio; mod testdata; -mod v2x_tx; fn main() { // env_logger::init(); diff --git a/src/ubx.rs b/src/ubx.rs index 934af7b..8e1f32e 100644 --- a/src/ubx.rs +++ b/src/ubx.rs @@ -90,9 +90,9 @@ impl<'a> TryFrom<&ublox::nav_pvt::proto27::NavPvtRef<'a>> for crate::applogic::G let latitude_deg = pos.lat; let longitude_deg = pos.lon; - #[allow(clippy::cast_precision_loss)] + #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)] let heading_deg = vel.heading as f32; - #[allow(clippy::cast_precision_loss)] + #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)] let speed_mps = vel.speed as f32; Ok(Self { diff --git a/src/v2x_tx/gn.rs b/src/v2x_tx/gn.rs deleted file mode 100644 index 4b2b0b9..0000000 --- a/src/v2x_tx/gn.rs +++ /dev/null @@ -1,94 +0,0 @@ -//! 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"), - ) -} - -pub 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 deleted file mode 100644 index ee48578..0000000 --- a/src/v2x_tx/mod.rs +++ /dev/null @@ -1,164 +0,0 @@ -//! V2X Transmission - -use alloc::vec::Vec; - -use c_its_parser::gn::{self as geonetworking, Encode}; - -pub mod gn; - -#[cfg(all(feature = "esp", feature = "cam_tx"))] -/// 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, - ) -> Result< - ( - geonetworking::en302636_4_1::ExtendedHeader, - Option, - u8, - ), - alloc::string::String, - >; - - /// 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 - } -}