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 059f47df56
5 changed files with 411 additions and 82 deletions
Download patch file Download diff file Expand all files Collapse all files

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(),
@ -86,7 +120,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: &name,
appearance: &appearance::UNKNOWN,
@ -99,10 +147,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:?}");
@ -113,6 +166,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,
@ -202,63 +270,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(any(feature = "cam", feature = "cam_tx"))]
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

@ -513,16 +513,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}"),
}
}
@ -530,33 +550,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 {