BLE: Add raw messages

src/applogic/mod.rs

@@ -118,6 +118,7 @@ pub struct TimingData {
     pub max_end_sec: Option<i16>,
 }
 
+#[cfg(feature = "spat")]
 impl TimingData {
     pub fn new(value: &map::SignalGroup, now: chrono::DateTime<chrono::Utc>) -> Option<Self> {
         let min = value

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,7 +17,9 @@ 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)); // TODO: this needs to be an array!
+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 {
@@ -82,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(),
@@ -120,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:?}");
@@ -134,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,
@@ -223,20 +250,23 @@ 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>) {
+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);
             }
         });
 

src/io.rs

@@ -454,11 +454,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;
 
@@ -469,12 +469,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,
@@ -482,7 +484,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()),
@@ -500,8 +502,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 {
@@ -529,11 +531,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;
 
@@ -552,7 +554,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;
@@ -564,6 +567,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());

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
 
@@ -508,17 +508,13 @@ async fn main(spawner: Spawner) -> ! {
                             transport: _,
                             ref etsi,
                         } => {
-                            state.handle_mapem(&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)));
-                                    });
+                                    ble::update(ble::BleSendable::MapemRaw(data));
                                 }
                                 Err(err) => warn!("Failed to create RawMsgRx: {err}"),
                             }
@@ -530,18 +526,14 @@ async fn main(spawner: Spawner) -> ! {
                             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) => {
-                                    critical_section::with(|cs| {
-                                        let ble_update_ref = ble::TX_DATA.borrow(cs);
-                                        ble_update_ref
-                                            .replace(Some(ble::BleSendable::SpatemRaw(data)));
-                                    });
+                                    ble::update(ble::BleSendable::SpatemRaw(data));
                                 }
                                 Err(err) => warn!("Failed to create RawMsgRx: {err}"),
                             }
@@ -551,37 +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()));
 
-                            // TODO: Send Raw message, but only when it has high-freq container
+                                // 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()));
 
-                            // TODO: Send Raw message
+                                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 {