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

BLE: Add raw messages

Browse source 
BLE can only fit 251 bytes of payload in notifications which isn't nearly enough for a raw V2X message.
So only publish a notification which message was received and let the client
poll the content from the characteristic (since reads can get data in fragments)
This commit is contained in:
Jannik Beyerstedt 2026-08-17 14:53:48 +02:00
commit 62997dbfcb
5 changed files with 411 additions and 82 deletions
Download patch file Download diff file Expand all files Collapse all files

33
docs/ble-protocol.md
View file

@ -19,12 +19,16 @@ 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-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)
c0b70001-d4f4-4002-ada8-f99a02ee315c | `RawNotify` | Raw message notification
c0b70001-d4f4-4003-ada8-f99a02ee315c | `RawMsgRx` | Raw CAM message (de-duplicated/ rate-limited)
c0b70001-d4f4-4004-ada8-f99a02ee315c | `CamEvent` | Latest received CAM
c0b70001-d4f4-4005-ada8-f99a02ee315c | `RawMsgRx` | Raw DENM message (de-duplicated/ rate-limited)
c0b70001-d4f4-4006-ada8-f99a02ee315c | `DenmEvent` | Latest received DENM
c0b70001-d4f4-4007-ada8-f99a02ee315c | `RawMsgRx` | Raw MAPEM message (de-duplicated/ rate-limited)
c0b70001-d4f4-4008-ada8-f99a02ee315c | `RawMsgRx` | Raw SPATEM message (de-duplicated/ rate-limited)
Since a modern BLE notification can only fit up tp 251 bytes of payload, the raw messages can only be polled.
Please subscribe to `c0b70001-d4f4-4002-ada8-f99a02ee315c` and use the message ID to determine from which service to read the incoming value.
## Message Format
@ -101,3 +105,20 @@ 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
### Raw Message Notification
Maximum message size (assuming realistic value ranges):
field name | tag size | (max.) data size
--------------------- | -------- | ---------
msg_type | 1 | 1 (VARINT(21))
**SUM** | 1 + | 1 = 2 byte

4
docs/c_its-messages.proto
View file

@ -71,6 +71,10 @@ message RawMsgTx {
optional uint32 stationType = 6; // for source position vector
}
message RawNotify {
required uint32 message_type = 1; // ITS `MessageId` integer value
}
// ------------------------------
// Messages
// ------------------------------

247
src/ble.rs
View file

@ -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<RefCell<Option<BleSendable>>> = Mutex::new(RefCell::new(None));
pub static CLIENT_CONNECTED: Mutex<RefCell<bool>> = Mutex::new(RefCell::new(false));
pub static TX_DATA: Mutex<RefCell<Option<ArrayQueue<BleSendable>>>> =
Mutex::new(RefCell::new(None));
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 +44,43 @@ 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(any(feature = "cam", feature = "denm", feature = "spat"))]
#[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "Raw_Notify", read, value = "Raw Message Notification", type = &'static str)]
#[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::RawNotify::PROTO_SIZE])]
raw_notify: [u8; io::msg::RawNotify::PROTO_SIZE],
#[cfg(feature = "cam")]
#[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM", read, value = "RawMsgRx proto msg", type = &'static str)]
#[characteristic(uuid = "c0b70001-d4f4-4003-ada8-f99a02ee315c", read, 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-4004-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-4005-ada8-f99a02ee315c", read, 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-4006-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-4007-ada8-f99a02ee315c", read, 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-4008-ada8-f99a02ee315c", read, value = [0u8; io::msg::RawMsgRx::PROTO_SIZE])]
spatem_raw: [u8; io::msg::RawMsgRx::PROTO_SIZE],
}
/// Creates the BLE controller
@ -60,6 +90,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(),
@ -85,7 +119,21 @@ pub async fn run(
..
} = stack.build();
info!("Starting advertising and GATT service");
let mut features = alloc::vec![];
if cfg!(feature = "cam") {
features.push("cam");
}
if cfg!(feature = "denm") {
features.push("denm");
}
if cfg!(feature = "spat") {
features.push("mapem");
features.push("spatem");
}
info!(
"Starting advertising and GATT service for {}",
features.join(",")
);
let server = Server::new_with_config(GapConfig::Peripheral(PeripheralConfig {
name: "ESP32 C-ITS",
appearance: &appearance::UNKNOWN,
@ -98,10 +146,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 +165,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,63 +269,142 @@ async fn gatt_events_task<P: PacketPool>(
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<P: PacketPool>(server: &Server<'_>, conn: &GattConnection<'_, '_, P>) {
#[allow(clippy::too_many_lines)]
async fn publishing_task<P: PacketPool>(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.set(server, &value) {
warn!("BLE: failed setting new raw CAM value: {err:?}");
}
if let Err(err) = send_raw_notify(
server,
conn,
c_its_parser::standards::extensions::ItsMessageId::Cam,
)
.await
{
warn!("BLE: {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.set(server, &value) {
warn!("BLE: failed setting new raw DENM value: {err:?}");
}
if let Err(err) = send_raw_notify(
server,
conn,
c_its_parser::standards::extensions::ItsMessageId::Denm,
)
.await
{
warn!("BLE: {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.set(server, &value) {
warn!("BLE: failed setting new raw MAPEM value: {err:?}");
}
if let Err(err) = send_raw_notify(
server,
conn,
c_its_parser::standards::extensions::ItsMessageId::Mapem,
)
.await
{
warn!("BLE: {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.set(server, &value) {
warn!("BLE: failed setting new raw SPATEM value: {err:?}");
}
if let Err(err) = send_raw_notify(
server,
conn,
c_its_parser::standards::extensions::ItsMessageId::Spatem,
)
.await
{
warn!("BLE: {err}");
}
}
Err(err) => warn!("BLE: Raw SPATEM {err}"),
},
None => {}
}
}
}
async fn send_raw_notify<P: PacketPool>(
server: &Server<'_>,
conn: &GattConnection<'_, '_, P>,
msg_id: c_its_parser::standards::extensions::ItsMessageId,
) -> Result<(), alloc::string::String> {
let notify_buf = io::msg::RawNotify::from(msg_id)
.serialize_ble_proto()
.expect("BLE: Raw notify too big");
server
.c_its_events
.raw_notify
.notify(conn, &notify_buf)
.await
.map_err(|err| alloc::format!("failed notifying connection of new raw {msg_id:?}: {err:?}"))
}

146
src/io.rs
View file

@ -40,20 +40,91 @@ 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(&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(&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(&mut mut_buf) {
Err(alloc::format!("Buffer too small: {err:?}"))
} else {
Ok(data)
}
}
}
#[cfg(all(
feature = "ble",
any(feature = "cam", feature = "denm", feature = "spat")
))]
impl From<c_its_parser::standards::extensions::ItsMessageId> for msg::RawNotify {
fn from(value: c_its_parser::standards::extensions::ItsMessageId) -> Self {
let message_type = value.as_u8().into();
Self { message_type }
}
}
#[cfg(any(feature = "cam", feature = "denm", feature = "spat"))]
impl msg::RawNotify {
#[cfg(feature = "ble")]
pub const PROTO_SIZE: usize = 2;
#[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(&mut mut_buf) {
Err(alloc::format!("Buffer too small: {err:?}"))
} else {
Ok(data)
}
}
}
@ -414,11 +485,11 @@ impl msg::RawMsgTx {
}
#[cfg(feature = "cam")]
impl From<alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::CAM>>
impl From<&alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::CAM>>
for msg::CamEvent
{
fn from(
value: alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::CAM>,
value: &alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::CAM>,
) -> Self {
use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions;
@ -429,12 +500,14 @@ impl From<alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descript
.cam_parameters
.basic_container
.reference_position
.clone()
.into();
let vehicle_role = value
.cam
.cam_parameters
.low_frequency_container
.as_ref()
.and_then(|v| match v {
cam_pdu_descriptions::LowFrequencyContainer::basicVehicleContainerLowFrequency(
basic_vehicle_container_low_frequency,
@ -442,7 +515,7 @@ impl From<alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descript
_ => 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 +533,8 @@ impl From<alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descript
}
#[cfg(feature = "cam")]
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 {
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 +562,11 @@ impl From<c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicle
}
#[cfg(feature = "denm")]
impl From<alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_description::DENM>>
impl From<&alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_description::DENM>>
for msg::DenmEvent
{
fn from(
value: alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_description::DENM>,
value: &alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_description::DENM>,
) -> Self {
let denm_mgmt = &value.denm.management;
@ -512,7 +585,8 @@ impl From<alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_descri
let event_heading_deg = value
.denm
.location
.and_then(|v| v.event_position_heading)
.as_ref()
.and_then(|v| v.event_position_heading.clone())
.map(|v| {
#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
let conv = v.value.as_deg() as u32;
@ -524,6 +598,7 @@ impl From<alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_descri
let cc_tuple = value
.denm
.situation
.as_ref()
.map(|v| v.event_type.cc_and_scc.to_u8_tuple());
let cause_code = cc_tuple.map(|v| v.0.into());
let sub_cause_code = cc_tuple.map(|v| v.1.into());
@ -592,6 +667,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 {

67
src/main.rs
View file

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