jtbx/esp32-c_its-companion
Watch
0
0
Fork
You've already forked esp32-c_its-companion
 0
Code Activity

WIP BLE: Add raw messages

Browse source 
TODO: make BLE TX queue an array
This commit is contained in:
Jannik Beyerstedt 2026-08-17 14:53:48 +02:00
commit b36913e40b
4 changed files with 212 additions and 45 deletions
Download patch file Download diff file Expand all files Collapse all files

8
docs/ble-protocol.md
View file

@ -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

116
src/ble.rs
View file

@ -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<RefCell<Option<BleSendable>>> = Mutex::new(RefCell::new(None));
pub static TX_DATA: Mutex<RefCell<Option<BleSendable>>> = Mutex::new(RefCell::new(None)); // TODO: this needs to be an array!
pub static RX_DATA: Mutex<RefCell<Option<io::msg::PositionState>>> = 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<P: PacketPool>(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 => {}
}
}

95
src/io.rs
View file

@ -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<u8> {
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<u8> {
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<c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection> 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<Self, Self::Error> {
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 {

38
src/main.rs
View file

@ -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 {