From 9b2002501400422ef93ab5acbc45daf986ab812b Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 17 Aug 2026 11:35:00 +0200 Subject: [PATCH 1/6] BLE: Fix UUIDs --- docs/ble-protocol.md | 12 ++++++------ src/ble.rs | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/docs/ble-protocol.md b/docs/ble-protocol.md index 4560424..8d742e2 100644 --- a/docs/ble-protocol.md +++ b/docs/ble-protocol.md @@ -19,12 +19,12 @@ V2X data is exposed via the service UUID `c0b70000-d4f4-4000-ada8-f99a02ee315c` UUID | Type | Description ------------------------------------- | --------------- | ----------- c0b70001-d4f4-4001-ada8-f99a02ee315c | `PositionState` | Set position from external source -c0b70001-d4f4-4001-ada8-f99a02ee315c | `RawMsgRx` | Raw CAM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4002-ada8-f99a02ee315c | `CamEvent` | Latest received CAM -c0b70001-d4f4-4003-ada8-f99a02ee315c | `RawMsgRx` | Raw DENM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4004-ada8-f99a02ee315c | `DenmEvent` | Latest received DENM -c0b70001-d4f4-4005-ada8-f99a02ee315c | `RawMsgRx` | Raw MAPEM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4006-ada8-f99a02ee315c | `RawMsgRx` | Raw SPATEM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4002-ada8-f99a02ee315c | `RawMsgRx` | Raw CAM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4003-ada8-f99a02ee315c | `CamEvent` | Latest received CAM +c0b70001-d4f4-4004-ada8-f99a02ee315c | `RawMsgRx` | Raw DENM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4005-ada8-f99a02ee315c | `DenmEvent` | Latest received DENM +c0b70001-d4f4-4006-ada8-f99a02ee315c | `RawMsgRx` | Raw MAPEM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4007-ada8-f99a02ee315c | `RawMsgRx` | Raw SPATEM message (de-duplicated/ rate-limited) ## Message Format diff --git a/src/ble.rs b/src/ble.rs index b585e86..78403f7 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -44,12 +44,12 @@ struct CITSEvents { #[cfg(feature = "cam")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM_Event", read, value = "CamEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])] + #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])] cam_event: [u8; CAM_EVENT_SIZE], #[cfg(feature = "denm")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM_Event", read, value = "DenmEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])] + #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])] denm_event: [u8; DENM_EVENT_SIZE], } From b36913e40bfa7ff6364ffa60c77111698010777b Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 17 Aug 2026 14:53:48 +0200 Subject: [PATCH 2/6] WIP BLE: Add raw messages TODO: make BLE TX queue an array --- docs/ble-protocol.md | 8 +++ src/ble.rs | 116 +++++++++++++++++++++++++++++-------------- src/io.rs | 95 +++++++++++++++++++++++++++++++++-- src/main.rs | 38 ++++++++++++-- 4 files changed, 212 insertions(+), 45 deletions(-) diff --git a/docs/ble-protocol.md b/docs/ble-protocol.md index 8d742e2..01448c6 100644 --- a/docs/ble-protocol.md +++ b/docs/ble-protocol.md @@ -101,3 +101,11 @@ Repetitions of the same message should be dropped by the BLE server. Consecutive messages with no relevant changes may be dropped by the BLE server while keeping some minimal publishing rate, e.g.: - only publish MAPEMs every 10 seconds (since content doesn't change, but client may be restarted after first MAPEM from an intersection was received) - drop high-frequency CAMs (only publish "full" CAMs which include the low-frequency container) + +Maximum message size (assuming realistic value ranges): + +field name | tag size | (max.) data size +--------------------- | -------- | --------- +msg_type | 1 | 1 (VARINT(21)) +payload | 1 | 2+1394 (LEN(1394) + payload) +**SUM** | 2 + | 1397 = 1399 byte diff --git a/src/ble.rs b/src/ble.rs index 78403f7..8fc86e5 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -16,14 +16,24 @@ pub const CONTROLLER_SLOTS: usize = 20; const CONNECTIONS_MAX: usize = 1; const L2CAP_CHANNELS_MAX: usize = 1; -pub static TX_DATA: Mutex>> = Mutex::new(RefCell::new(None)); +pub static TX_DATA: Mutex>> = Mutex::new(RefCell::new(None)); // TODO: this needs to be an array! pub static RX_DATA: Mutex>> = Mutex::new(RefCell::new(None)); pub enum BleSendable { + #[cfg(feature = "cam")] + CamRaw(io::msg::RawMsgRx), #[cfg(feature = "cam")] Cam(io::msg::CamEvent), + + #[cfg(feature = "denm")] + DenmRaw(io::msg::RawMsgRx), #[cfg(feature = "denm")] Denm(io::msg::DenmEvent), + + #[cfg(feature = "spat")] + MapemRaw(io::msg::RawMsgRx), + #[cfg(feature = "spat")] + SpatemRaw(io::msg::RawMsgRx), } #[gatt_server] @@ -31,26 +41,38 @@ struct Server { c_its_events: CITSEvents, } -#[cfg(feature = "cam")] -const CAM_EVENT_SIZE: usize = 38; -#[cfg(feature = "denm")] -const DENM_EVENT_SIZE: usize = 51; - #[gatt_service(uuid = "c0b70000-d4f4-4000-ada8-f99a02ee315c")] struct CITSEvents { #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "Position", read, value = "PositionState proto msg", type = &'static str)] #[characteristic(uuid = "c0b70001-d4f4-4001-ada8-f99a02ee315c", write)] position: [u8; 27], + #[cfg(feature = "cam")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + cam_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], #[cfg(feature = "cam")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM_Event", read, value = "CamEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])] - cam_event: [u8; CAM_EVENT_SIZE], + #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::CamEvent::PROTO_SIZE])] + cam_event: [u8; io::msg::CamEvent::PROTO_SIZE], + #[cfg(feature = "denm")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + denm_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], #[cfg(feature = "denm")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM_Event", read, value = "DenmEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])] - denm_event: [u8; DENM_EVENT_SIZE], + #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::DenmEvent::PROTO_SIZE])] + denm_event: [u8; io::msg::DenmEvent::PROTO_SIZE], + + #[cfg(feature = "spat")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "MAPEM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4006-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + mapem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], + #[cfg(feature = "spat")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "SPATEM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4007-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + spatem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], } /// Creates the BLE controller @@ -220,43 +242,61 @@ async fn custom_task(server: &Server<'_>, conn: &GattConnection<' match new_data { #[cfg(feature = "cam")] - Some(BleSendable::Cam(cam_event)) => { - let mut buf = cam_event.serialize_ble_proto(); - - if buf.len() <= CAM_EVENT_SIZE { - // pad with zeros - let pad = alloc::vec![0u8; CAM_EVENT_SIZE - buf.len()]; - buf.extend_from_slice(&pad); - - // unwrap should be fine since we ensured the vector size before - let value = buf.try_into().unwrap(); - + Some(BleSendable::Cam(cam_event)) => match cam_event.serialize_ble_proto() { + Ok(value) => { if let Err(err) = server.c_its_events.cam_event.notify(conn, &value).await { warn!("BLE: failed notifying connection of new CAM event: {err:?}"); } - } else { - warn!("BLE: CAM event too big with {} bytes", buf.len()); } - } + Err(err) => warn!("BLE: CAM event {err}"), + }, + #[cfg(feature = "denm")] - Some(BleSendable::Denm(denm_event)) => { - let mut buf = denm_event.serialize_ble_proto(); - - if buf.len() <= DENM_EVENT_SIZE { - // pad with zeros - let pad = alloc::vec![0u8; DENM_EVENT_SIZE - buf.len()]; - buf.extend_from_slice(&pad); - - // unwrap should be fine since we ensured the vector size before - let value = buf.try_into().unwrap(); - + Some(BleSendable::Denm(denm_event)) => match denm_event.serialize_ble_proto() { + Ok(value) => { if let Err(err) = server.c_its_events.denm_event.notify(conn, &value).await { warn!("BLE: failed notifying connection of new DENM event: {err:?}"); } - } else { - warn!("BLE: DENM event too big with {} bytes", buf.len()); } - } + Err(err) => warn!("BLE: DENM event {err}"), + }, + + #[cfg(feature = "cam")] + Some(BleSendable::CamRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.cam_raw.notify(conn, &value).await { + warn!("BLE: failed notifying connection of new raw CAM: {err:?}"); + } + } + Err(err) => warn!("BLE: Raw CAM too big: {err}"), + }, + #[cfg(feature = "denm")] + Some(BleSendable::DenmRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.denm_raw.notify(conn, &value).await { + warn!("BLE: failed notifying connection of new raw DENM: {err:?}"); + } + } + Err(err) => warn!("BLE: Raw DENM {err}"), + }, + #[cfg(feature = "spat")] + Some(BleSendable::MapemRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.mapem_raw.notify(conn, &value).await { + warn!("BLE: failed notifying connection of new raw MAPEM: {err:?}"); + } + } + Err(err) => warn!("BLE: Raw MAPEM {err}"), + }, + #[cfg(feature = "spat")] + Some(BleSendable::SpatemRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.spatem_raw.notify(conn, &value).await { + warn!("BLE: failed notifying connection of new raw SPATEM: {err:?}"); + } + } + Err(err) => warn!("BLE: Raw SPATEM {err}"), + }, None => {} } } diff --git a/src/io.rs b/src/io.rs index 3ef8c6d..aeeab92 100644 --- a/src/io.rs +++ b/src/io.rs @@ -40,20 +40,60 @@ impl msg::RawMsgRx { #[cfg(feature = "denm")] impl msg::DenmEvent { #[cfg(feature = "ble")] - pub fn serialize_ble_proto(&self) -> alloc::vec::Vec { + pub const PROTO_SIZE: usize = 51; + + #[cfg(feature = "ble")] + pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> { use prost::Message as _; - self.encode_to_vec() + let mut data = [0u8; Self::PROTO_SIZE]; + + let mut mut_buf = &mut data[..]; + if let Err(err) = self.encode_length_delimited(&mut mut_buf) { + Err(alloc::format!("Buffer too small: {err:?}")) + } else { + Ok(data) + } } } #[cfg(feature = "cam")] impl msg::CamEvent { #[cfg(feature = "ble")] - pub fn serialize_ble_proto(&self) -> alloc::vec::Vec { + pub const PROTO_SIZE: usize = 38; + + #[cfg(feature = "ble")] + pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> { use prost::Message as _; - self.encode_to_vec() + let mut data = [0u8; Self::PROTO_SIZE]; + + let mut mut_buf = &mut data[..]; + if let Err(err) = self.encode_length_delimited(&mut mut_buf) { + Err(alloc::format!("Buffer too small: {err:?}")) + } else { + Ok(data) + } + } +} + +#[cfg(any(feature = "cam", feature = "denm", feature = "spat"))] +impl msg::RawMsgRx { + #[cfg(feature = "ble")] + pub const PROTO_SIZE: usize = 1400; + + #[cfg(feature = "ble")] + pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> { + use prost::Message as _; + + let mut data = [0u8; Self::PROTO_SIZE]; + + let mut mut_buf = &mut data[..]; + if let Err(err) = self.encode_length_delimited(&mut mut_buf) { + Err(alloc::format!("Buffer too small: {err:?}")) + } else { + Ok(data) + } } } @@ -592,6 +632,53 @@ impl From fo } } +#[cfg(any(feature = "cam", feature = "denm", feature = "spat"))] +impl TryFrom<&c_its_parser::ItsMessage<'_>> for msg::RawMsgRx { + type Error = alloc::string::String; + + fn try_from(value: &c_its_parser::ItsMessage<'_>) -> Result { + let msg_id = c_its_parser::standards::extensions::ItsMessageId::from(value); + + let payload = match value { + #[cfg(feature = "denm")] + c_its_parser::ItsMessage::DenmV1 { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(feature = "denm")] + c_its_parser::ItsMessage::DenmV2 { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(feature = "cam")] + c_its_parser::ItsMessage::Cam { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(feature = "spat")] + c_its_parser::ItsMessage::Spatem { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(feature = "spat")] + c_its_parser::ItsMessage::Mapem { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + }?; + + Ok(Self { + msg_type: msg_id.as_u8().into(), + payload, + }) + } +} + #[cfg(test)] mod tests { diff --git a/src/main.rs b/src/main.rs index 5e4e0c5..451ec21 100644 --- a/src/main.rs +++ b/src/main.rs @@ -506,17 +506,45 @@ async fn main(spawner: Spawner) -> ! { ItsMessage::Mapem { geonetworking: _, transport: _, - etsi, - } => state.handle_mapem(&etsi), + ref etsi, + } => { + state.handle_mapem(&etsi); + + // Send data to BLE thread + #[cfg(feature = "ble")] + match io::msg::RawMsgRx::try_from(&msg) { + Ok(data) => { + critical_section::with(|cs| { + let ble_update_ref = ble::TX_DATA.borrow(cs); + ble_update_ref + .replace(Some(ble::BleSendable::MapemRaw(data))); + }); + } + Err(err) => warn!("Failed to create RawMsgRx: {err}"), + } + } #[cfg(feature = "spat")] ItsMessage::Spatem { geonetworking: _, transport: _, - etsi, + ref etsi, } => { if state.initialized() { state.handle_spatem(&etsi); } + + // Send data to BLE thread + #[cfg(feature = "ble")] + match io::msg::RawMsgRx::try_from(&msg) { + Ok(data) => { + critical_section::with(|cs| { + let ble_update_ref = ble::TX_DATA.borrow(cs); + ble_update_ref + .replace(Some(ble::BleSendable::SpatemRaw(data))); + }); + } + Err(err) => warn!("Failed to create RawMsgRx: {err}"), + } } #[cfg(any(feature = "cam", feature = "cam_tx"))] @@ -534,6 +562,8 @@ async fn main(spawner: Spawner) -> ! { let ble_update_ref = ble::TX_DATA.borrow(cs); ble_update_ref.replace(Some(ble::BleSendable::Cam(etsi.into()))); }); + + // TODO: Send Raw message, but only when it has high-freq container } #[cfg(feature = "denm")] @@ -550,6 +580,8 @@ async fn main(spawner: Spawner) -> ! { let ble_update_ref = ble::TX_DATA.borrow(cs); ble_update_ref.replace(Some(ble::BleSendable::Denm(etsi.into()))); }); + + // TODO: Send Raw message } #[cfg(feature = "denm")] ItsMessage::DenmV1 { From a2b0819fb011a4a333bbbdc5b2bd806f05f0f0a1 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 17 Aug 2026 15:00:00 +0200 Subject: [PATCH 3/6] main: Tweak vehicle size --- src/main.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main.rs b/src/main.rs index 5e4e0c5..6eb670c 100644 --- a/src/main.rs +++ b/src/main.rs @@ -68,9 +68,9 @@ 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! +const OWN_VEHICLE_WIDTH: u8 = 7; // in 10cm steps! #[cfg(feature = "cam_tx")] -const OWN_VEHICLE_LENGTH: u16 = 20; // in 10cm steps! +const OWN_VEHICLE_LENGTH: u16 = 18; // in 10cm steps! #[cfg(feature = "_uart")] const SERIAL_FIFO_SIZE_THLD: u8 = 100; // hardware has max. 128 byte From 352ef6fb7a1c945f4fddf8d7b8942b8a567daecc Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 17 Aug 2026 15:20:49 +0200 Subject: [PATCH 4/6] applogic: Add missing feature flag for `TimingData` impl --- src/applogic/mod.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/applogic/mod.rs b/src/applogic/mod.rs index 55672a7..0ced524 100644 --- a/src/applogic/mod.rs +++ b/src/applogic/mod.rs @@ -118,6 +118,7 @@ pub struct TimingData { pub max_end_sec: Option, } +#[cfg(feature = "spat")] impl TimingData { pub fn new(value: &map::SignalGroup, now: chrono::DateTime) -> Option { let min = value From 51ebd75cd0bcd4c3519b52ba0ed1e41a4c1a8818 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 17 Aug 2026 11:35:00 +0200 Subject: [PATCH 5/6] BLE: Fix UUIDs --- docs/ble-protocol.md | 12 ++++++------ src/ble.rs | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/docs/ble-protocol.md b/docs/ble-protocol.md index 4560424..8d742e2 100644 --- a/docs/ble-protocol.md +++ b/docs/ble-protocol.md @@ -19,12 +19,12 @@ V2X data is exposed via the service UUID `c0b70000-d4f4-4000-ada8-f99a02ee315c` UUID | Type | Description ------------------------------------- | --------------- | ----------- c0b70001-d4f4-4001-ada8-f99a02ee315c | `PositionState` | Set position from external source -c0b70001-d4f4-4001-ada8-f99a02ee315c | `RawMsgRx` | Raw CAM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4002-ada8-f99a02ee315c | `CamEvent` | Latest received CAM -c0b70001-d4f4-4003-ada8-f99a02ee315c | `RawMsgRx` | Raw DENM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4004-ada8-f99a02ee315c | `DenmEvent` | Latest received DENM -c0b70001-d4f4-4005-ada8-f99a02ee315c | `RawMsgRx` | Raw MAPEM message (de-duplicated/ rate-limited) -c0b70001-d4f4-4006-ada8-f99a02ee315c | `RawMsgRx` | Raw SPATEM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4002-ada8-f99a02ee315c | `RawMsgRx` | Raw CAM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4003-ada8-f99a02ee315c | `CamEvent` | Latest received CAM +c0b70001-d4f4-4004-ada8-f99a02ee315c | `RawMsgRx` | Raw DENM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4005-ada8-f99a02ee315c | `DenmEvent` | Latest received DENM +c0b70001-d4f4-4006-ada8-f99a02ee315c | `RawMsgRx` | Raw MAPEM message (de-duplicated/ rate-limited) +c0b70001-d4f4-4007-ada8-f99a02ee315c | `RawMsgRx` | Raw SPATEM message (de-duplicated/ rate-limited) ## Message Format diff --git a/src/ble.rs b/src/ble.rs index b585e86..78403f7 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -44,12 +44,12 @@ struct CITSEvents { #[cfg(feature = "cam")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM_Event", read, value = "CamEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])] + #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])] cam_event: [u8; CAM_EVENT_SIZE], #[cfg(feature = "denm")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM_Event", read, value = "DenmEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])] + #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])] denm_event: [u8; DENM_EVENT_SIZE], } From b038ddff49fb8ed9e1a83f41655e9ddae7e768a4 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 17 Aug 2026 14:53:48 +0200 Subject: [PATCH 6/6] BLE: Add raw messages --- docs/ble-protocol.md | 8 +++ src/ble.rs | 168 ++++++++++++++++++++++++++++++------------- src/io.rs | 115 +++++++++++++++++++++++++---- src/main.rs | 67 ++++++++++++----- 4 files changed, 281 insertions(+), 77 deletions(-) diff --git a/docs/ble-protocol.md b/docs/ble-protocol.md index 8d742e2..01448c6 100644 --- a/docs/ble-protocol.md +++ b/docs/ble-protocol.md @@ -101,3 +101,11 @@ Repetitions of the same message should be dropped by the BLE server. Consecutive messages with no relevant changes may be dropped by the BLE server while keeping some minimal publishing rate, e.g.: - only publish MAPEMs every 10 seconds (since content doesn't change, but client may be restarted after first MAPEM from an intersection was received) - drop high-frequency CAMs (only publish "full" CAMs which include the low-frequency container) + +Maximum message size (assuming realistic value ranges): + +field name | tag size | (max.) data size +--------------------- | -------- | --------- +msg_type | 1 | 1 (VARINT(21)) +payload | 1 | 2+1394 (LEN(1394) + payload) +**SUM** | 2 + | 1397 = 1399 byte diff --git a/src/ble.rs b/src/ble.rs index 78403f7..e58ffb0 100644 --- a/src/ble.rs +++ b/src/ble.rs @@ -6,6 +6,7 @@ use core::cell::RefCell; use critical_section::Mutex; +use crossbeam_queue::ArrayQueue; use esp_backtrace as _; use log::{info, warn}; use trouble_host::prelude::*; @@ -16,14 +17,26 @@ pub const CONTROLLER_SLOTS: usize = 20; const CONNECTIONS_MAX: usize = 1; const L2CAP_CHANNELS_MAX: usize = 1; -pub static TX_DATA: Mutex>> = Mutex::new(RefCell::new(None)); +pub static CLIENT_CONNECTED: Mutex> = Mutex::new(RefCell::new(false)); +pub static TX_DATA: Mutex>>> = + Mutex::new(RefCell::new(None)); pub static RX_DATA: Mutex>> = Mutex::new(RefCell::new(None)); pub enum BleSendable { + #[cfg(feature = "cam")] + CamRaw(io::msg::RawMsgRx), #[cfg(feature = "cam")] Cam(io::msg::CamEvent), + + #[cfg(feature = "denm")] + DenmRaw(io::msg::RawMsgRx), #[cfg(feature = "denm")] Denm(io::msg::DenmEvent), + + #[cfg(feature = "spat")] + MapemRaw(io::msg::RawMsgRx), + #[cfg(feature = "spat")] + SpatemRaw(io::msg::RawMsgRx), } #[gatt_server] @@ -31,26 +44,38 @@ struct Server { c_its_events: CITSEvents, } -#[cfg(feature = "cam")] -const CAM_EVENT_SIZE: usize = 38; -#[cfg(feature = "denm")] -const DENM_EVENT_SIZE: usize = 51; - #[gatt_service(uuid = "c0b70000-d4f4-4000-ada8-f99a02ee315c")] struct CITSEvents { #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "Position", read, value = "PositionState proto msg", type = &'static str)] #[characteristic(uuid = "c0b70001-d4f4-4001-ada8-f99a02ee315c", write)] position: [u8; 27], + #[cfg(feature = "cam")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + cam_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], #[cfg(feature = "cam")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM_Event", read, value = "CamEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])] - cam_event: [u8; CAM_EVENT_SIZE], + #[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::CamEvent::PROTO_SIZE])] + cam_event: [u8; io::msg::CamEvent::PROTO_SIZE], + #[cfg(feature = "denm")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + denm_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], #[cfg(feature = "denm")] #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM_Event", read, value = "DenmEvent proto msg", type = &'static str)] - #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])] - denm_event: [u8; DENM_EVENT_SIZE], + #[characteristic(uuid = "c0b70001-d4f4-4005-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::DenmEvent::PROTO_SIZE])] + denm_event: [u8; io::msg::DenmEvent::PROTO_SIZE], + + #[cfg(feature = "spat")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "MAPEM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4006-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + mapem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], + #[cfg(feature = "spat")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "SPATEM", read, value = "RawMsgRx proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4007-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])] + spatem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE], } /// Creates the BLE controller @@ -60,6 +85,10 @@ pub fn make_controller( esp_radio::ble::controller::BleConnector<'static>, CONTROLLER_SLOTS, > { + critical_section::with(|cs| { + TX_DATA.borrow(cs).replace(Some(ArrayQueue::new(2))); + }); + let transport = esp_radio::ble::controller::BleConnector::new( bt_peripheral, esp_radio::ble::Config::default(), @@ -98,10 +127,15 @@ pub async fn run( Ok(conn) => { // set up tasks when the connection is established to a central, so they don't run when no one is connected. let a = gatt_events_task(&server, &conn); - let b = custom_task(&server, &conn); - // run until any task ends (usually because the connection has been closed), - // then return to advertising state. + let b = publishing_task(&server, &conn); + + // run until any task ends (usually because the connection has been closed), then return to advertising state. embassy_futures::select::select(a, b).await; + + critical_section::with(|cs| { + let value_ref = CLIENT_CONNECTED.borrow(cs); + let _ = value_ref.replace(false); + }); } Err(e) => { panic!("BLE ADV: fatal error {e:?}"); @@ -112,6 +146,21 @@ pub async fn run( .await; } +pub fn update(payload: BleSendable) { + critical_section::with(|cs| { + // only add to queue when someone is connected + if *CLIENT_CONNECTED.borrow(cs).borrow() { + let queue_rc = TX_DATA.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).is_err() { + log::error!("BLE TX queue is full"); + } + } + }); +} + /// Create an advertiser to use to connect to a BLE Central, and wait for it to connect. async fn advertise<'values, 'server, C: Controller>( name: &'values str, @@ -201,62 +250,83 @@ async fn gatt_events_task( Ok(()) } -/// Example task to use the BLE notifier interface. -/// This task will notify the connected central of a counter value every 2 seconds. -/// It will also read the RSSI value every 2 seconds. -/// and will stop when the connection is closed by the central or an error occurs. -async fn custom_task(server: &Server<'_>, conn: &GattConnection<'_, '_, P>) { +async fn publishing_task(server: &Server<'_>, conn: &GattConnection<'_, '_, P>) { + critical_section::with(|cs| { + let value_ref = CLIENT_CONNECTED.borrow(cs); + let _ = value_ref.replace(true); + }); + loop { embassy_time::Timer::after(embassy_time::Duration::from_millis(10)).await; let mut new_data = None; critical_section::with(|cs| { - let tx_data_ref = TX_DATA.borrow(cs); + let queue_rc = TX_DATA.borrow(cs).borrow(); + // unwrap is fine b/c we stored something in it before + let queue = queue_rc.as_ref().unwrap(); - if let Some(tx_data) = tx_data_ref.replace(None) { - new_data = Some(tx_data); + if let Some(data) = queue.pop() { + new_data = Some(data); } }); match new_data { #[cfg(feature = "cam")] - Some(BleSendable::Cam(cam_event)) => { - let mut buf = cam_event.serialize_ble_proto(); - - if buf.len() <= CAM_EVENT_SIZE { - // pad with zeros - let pad = alloc::vec![0u8; CAM_EVENT_SIZE - buf.len()]; - buf.extend_from_slice(&pad); - - // unwrap should be fine since we ensured the vector size before - let value = buf.try_into().unwrap(); - + Some(BleSendable::Cam(cam_event)) => match cam_event.serialize_ble_proto() { + Ok(value) => { if let Err(err) = server.c_its_events.cam_event.notify(conn, &value).await { warn!("BLE: failed notifying connection of new CAM event: {err:?}"); } - } else { - warn!("BLE: CAM event too big with {} bytes", buf.len()); } - } + Err(err) => warn!("BLE: CAM event {err}"), + }, + #[cfg(feature = "denm")] - Some(BleSendable::Denm(denm_event)) => { - let mut buf = denm_event.serialize_ble_proto(); - - if buf.len() <= DENM_EVENT_SIZE { - // pad with zeros - let pad = alloc::vec![0u8; DENM_EVENT_SIZE - buf.len()]; - buf.extend_from_slice(&pad); - - // unwrap should be fine since we ensured the vector size before - let value = buf.try_into().unwrap(); - + Some(BleSendable::Denm(denm_event)) => match denm_event.serialize_ble_proto() { + Ok(value) => { if let Err(err) = server.c_its_events.denm_event.notify(conn, &value).await { warn!("BLE: failed notifying connection of new DENM event: {err:?}"); } - } else { - warn!("BLE: DENM event too big with {} bytes", buf.len()); } - } + Err(err) => warn!("BLE: DENM event {err}"), + }, + + #[cfg(feature = "cam")] + Some(BleSendable::CamRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.cam_raw.notify(conn, &value).await { + warn!("BLE: failed notifying connection of new raw CAM: {err:?}"); + } + } + Err(err) => warn!("BLE: Raw CAM too big: {err}"), + }, + #[cfg(feature = "denm")] + Some(BleSendable::DenmRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.denm_raw.notify(conn, &value).await { + warn!("BLE: failed notifying connection of new raw DENM: {err:?}"); + } + } + Err(err) => warn!("BLE: Raw DENM {err}"), + }, + #[cfg(feature = "spat")] + Some(BleSendable::MapemRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.mapem_raw.notify(conn, &value).await { + warn!("BLE: failed notifying connection of new raw MAPEM: {err:?}"); + } + } + Err(err) => warn!("BLE: Raw MAPEM {err}"), + }, + #[cfg(feature = "spat")] + Some(BleSendable::SpatemRaw(raw_msg)) => match raw_msg.serialize_ble_proto() { + Ok(value) => { + if let Err(err) = server.c_its_events.spatem_raw.notify(conn, &value).await { + warn!("BLE: failed notifying connection of new raw SPATEM: {err:?}"); + } + } + Err(err) => warn!("BLE: Raw SPATEM {err}"), + }, None => {} } } diff --git a/src/io.rs b/src/io.rs index 3ef8c6d..1241953 100644 --- a/src/io.rs +++ b/src/io.rs @@ -40,20 +40,60 @@ impl msg::RawMsgRx { #[cfg(feature = "denm")] impl msg::DenmEvent { #[cfg(feature = "ble")] - pub fn serialize_ble_proto(&self) -> alloc::vec::Vec { + pub const PROTO_SIZE: usize = 51; + + #[cfg(feature = "ble")] + pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> { use prost::Message as _; - self.encode_to_vec() + let mut data = [0u8; Self::PROTO_SIZE]; + + let mut mut_buf = &mut data[..]; + if let Err(err) = self.encode_length_delimited(&mut mut_buf) { + Err(alloc::format!("Buffer too small: {err:?}")) + } else { + Ok(data) + } } } #[cfg(feature = "cam")] impl msg::CamEvent { #[cfg(feature = "ble")] - pub fn serialize_ble_proto(&self) -> alloc::vec::Vec { + pub const PROTO_SIZE: usize = 38; + + #[cfg(feature = "ble")] + pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> { use prost::Message as _; - self.encode_to_vec() + let mut data = [0u8; Self::PROTO_SIZE]; + + let mut mut_buf = &mut data[..]; + if let Err(err) = self.encode_length_delimited(&mut mut_buf) { + Err(alloc::format!("Buffer too small: {err:?}")) + } else { + Ok(data) + } + } +} + +#[cfg(any(feature = "cam", feature = "denm", feature = "spat"))] +impl msg::RawMsgRx { + #[cfg(feature = "ble")] + pub const PROTO_SIZE: usize = 1400; + + #[cfg(feature = "ble")] + pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> { + use prost::Message as _; + + let mut data = [0u8; Self::PROTO_SIZE]; + + let mut mut_buf = &mut data[..]; + if let Err(err) = self.encode_length_delimited(&mut mut_buf) { + Err(alloc::format!("Buffer too small: {err:?}")) + } else { + Ok(data) + } } } @@ -414,11 +454,11 @@ impl msg::RawMsgTx { } #[cfg(feature = "cam")] -impl From> +impl From<&alloc::boxed::Box> for msg::CamEvent { fn from( - value: alloc::boxed::Box, + value: &alloc::boxed::Box, ) -> Self { use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions; @@ -429,12 +469,14 @@ impl From None, }); - let vehicle_data = match value.cam.cam_parameters.high_frequency_container { + let vehicle_data = match &value.cam.cam_parameters.high_frequency_container { cam_pdu_descriptions::HighFrequencyContainer::basicVehicleContainerHighFrequency( basic_vehicle_container_high_frequency, ) => Some(basic_vehicle_container_high_frequency.into()), @@ -460,8 +502,8 @@ impl From for msg::VehicleContainer { - fn from(value: c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency) -> Self { +impl From<&c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency> for msg::VehicleContainer { + fn from(value: &c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency) -> Self { let heading_deg = if value.heading.heading_value.is_unavailable() { None } else { @@ -489,11 +531,11 @@ impl From> +impl From<&alloc::boxed::Box> for msg::DenmEvent { fn from( - value: alloc::boxed::Box, + value: &alloc::boxed::Box, ) -> Self { let denm_mgmt = &value.denm.management; @@ -512,7 +554,8 @@ impl From fo } } +#[cfg(any(feature = "cam", feature = "denm", feature = "spat"))] +impl TryFrom<&c_its_parser::ItsMessage<'_>> for msg::RawMsgRx { + type Error = alloc::string::String; + + fn try_from(value: &c_its_parser::ItsMessage<'_>) -> Result { + let msg_id = c_its_parser::standards::extensions::ItsMessageId::from(value); + + let payload = match value { + #[cfg(feature = "denm")] + c_its_parser::ItsMessage::DenmV1 { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(feature = "denm")] + c_its_parser::ItsMessage::DenmV2 { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(feature = "cam")] + c_its_parser::ItsMessage::Cam { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(feature = "spat")] + c_its_parser::ItsMessage::Spatem { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + #[cfg(feature = "spat")] + c_its_parser::ItsMessage::Mapem { + geonetworking: _, + transport: _, + etsi, + } => etsi.encode_to_uper(), + }?; + + Ok(Self { + msg_type: msg_id.as_u8().into(), + payload, + }) + } +} + #[cfg(test)] mod tests { diff --git a/src/main.rs b/src/main.rs index 6eb670c..d69a9ba 100644 --- a/src/main.rs +++ b/src/main.rs @@ -506,16 +506,36 @@ async fn main(spawner: Spawner) -> ! { ItsMessage::Mapem { geonetworking: _, transport: _, - etsi, - } => state.handle_mapem(&etsi), + ref etsi, + } => { + state.handle_mapem(etsi); + + // Send data to BLE thread + #[cfg(feature = "ble")] + match io::msg::RawMsgRx::try_from(&msg) { + Ok(data) => { + ble::update(ble::BleSendable::MapemRaw(data)); + } + Err(err) => warn!("Failed to create RawMsgRx: {err}"), + } + } #[cfg(feature = "spat")] ItsMessage::Spatem { geonetworking: _, transport: _, - etsi, + ref etsi, } => { if state.initialized() { - state.handle_spatem(&etsi); + state.handle_spatem(etsi); + } + + // Send data to BLE thread + #[cfg(feature = "ble")] + match io::msg::RawMsgRx::try_from(&msg) { + Ok(data) => { + ble::update(ble::BleSendable::SpatemRaw(data)); + } + Err(err) => warn!("Failed to create RawMsgRx: {err}"), } } @@ -523,33 +543,48 @@ async fn main(spawner: Spawner) -> ! { ItsMessage::Cam { geonetworking: _, transport: _, - etsi, + ref etsi, } => { #[cfg(feature = "cam")] - applogic::cam::handle_cam(&etsi); + applogic::cam::handle_cam(etsi); // Send data to BLE thread #[cfg(all(feature = "ble", feature = "cam"))] - critical_section::with(|cs| { - let ble_update_ref = ble::TX_DATA.borrow(cs); - ble_update_ref.replace(Some(ble::BleSendable::Cam(etsi.into()))); - }); + { + ble::update(ble::BleSendable::Cam(etsi.into())); + + // raw data only when it's a full CAM + if etsi.cam.cam_parameters.low_frequency_container.is_some() { + match io::msg::RawMsgRx::try_from(&msg) { + Ok(data) => { + ble::update(ble::BleSendable::CamRaw(data)); + } + Err(err) => warn!("Failed to create RawMsgRx: {err}"), + } + } + } } #[cfg(feature = "denm")] ItsMessage::DenmV2 { geonetworking: _, transport: _, - etsi, + ref etsi, } => { - applogic::denm::handle_denm(&etsi); + applogic::denm::handle_denm(etsi); // Send data to BLE thread #[cfg(feature = "ble")] - critical_section::with(|cs| { - let ble_update_ref = ble::TX_DATA.borrow(cs); - ble_update_ref.replace(Some(ble::BleSendable::Denm(etsi.into()))); - }); + { + ble::update(ble::BleSendable::Denm(etsi.into())); + + match io::msg::RawMsgRx::try_from(&msg) { + Ok(data) => { + ble::update(ble::BleSendable::DenmRaw(data)); + } + Err(err) => warn!("Failed to create RawMsgRx: {err}"), + } + } } #[cfg(feature = "denm")] ItsMessage::DenmV1 {