WIP BLE: Add raw messages

TODO: make BLE TX queue an array

src/applogic/mod.rs

@@ -118,7 +118,6 @@ 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,7 +6,6 @@
 use core::cell::RefCell;
 
 use critical_section::Mutex;
-use crossbeam_queue::ArrayQueue;
 use esp_backtrace as _;
 use log::{info, warn};
 use trouble_host::prelude::*;
@@ -17,9 +16,7 @@ pub const CONTROLLER_SLOTS: usize = 20;
 const CONNECTIONS_MAX: usize = 1;
 const L2CAP_CHANNELS_MAX: usize = 1;
 
-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 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 {
@@ -85,10 +82,6 @@ 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(),
@@ -127,15 +120,10 @@ 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 = publishing_task(&server, &conn);
-
-                    // run until any task ends (usually because the connection has been closed), then return to advertising state.
+                    let b = custom_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:?}");
@@ -146,21 +134,6 @@ 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,
@@ -250,23 +223,20 @@ async fn gatt_events_task<P: PacketPool>(
     Ok(())
 }
 
-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);
-    });
-
+/// 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>) {
     loop {
         embassy_time::Timer::after(embassy_time::Duration::from_millis(10)).await;
 
         let mut new_data = None;
         critical_section::with(|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();
+            let tx_data_ref = TX_DATA.borrow(cs);
 
-            if let Some(data) = queue.pop() {
-                new_data = Some(data);
+            if let Some(tx_data) = tx_data_ref.replace(None) {
+                new_data = Some(tx_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,14 +469,12 @@ impl From<&alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descrip
             .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,
@@ -484,7 +482,7 @@ impl From<&alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descrip
                 _ => 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()),
@@ -502,8 +500,8 @@ impl From<&alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descrip
 }
 
 #[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 {
@@ -531,11 +529,11 @@ impl From<&c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicl
 }
 
 #[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;
 
@@ -554,8 +552,7 @@ impl From<&alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_descr
         let event_heading_deg = value
             .denm
             .location
-            .as_ref()
-            .and_then(|v| v.event_position_heading.clone())
+            .and_then(|v| v.event_position_heading)
             .map(|v| {
                 #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
                 let conv = v.value.as_deg() as u32;
@@ -567,7 +564,6 @@ impl From<&alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_descr
         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 = 7; // in 10cm steps!
+const OWN_VEHICLE_WIDTH: u8 = 5; // in 10cm steps!
 #[cfg(feature = "cam_tx")]
-const OWN_VEHICLE_LENGTH: u16 = 18; // in 10cm steps!
+const OWN_VEHICLE_LENGTH: u16 = 20; // in 10cm steps!
 #[cfg(feature = "_uart")]
 const SERIAL_FIFO_SIZE_THLD: u8 = 100; // hardware has max. 128 byte
 
@@ -508,13 +508,17 @@ 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) => {
-                                    ble::update(ble::BleSendable::MapemRaw(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}"),
                             }
@@ -526,14 +530,18 @@ 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) => {
-                                    ble::update(ble::BleSendable::SpatemRaw(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}"),
                             }
@@ -543,48 +551,37 @@ async fn main(spawner: Spawner) -> ! {
                         ItsMessage::Cam {
                             geonetworking: _,
                             transport: _,
-                            ref etsi,
+                            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"))]
-                            {
-                                ble::update(ble::BleSendable::Cam(etsi.into()));
+                            critical_section::with(|cs| {
+                                let ble_update_ref = ble::TX_DATA.borrow(cs);
+                                ble_update_ref.replace(Some(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}"),
-                                    }
-                                }
-                            }
+                            // TODO: Send Raw message, but only when it has high-freq container
                         }
 
                         #[cfg(feature = "denm")]
                         ItsMessage::DenmV2 {
                             geonetworking: _,
                             transport: _,
-                            ref etsi,
+                            etsi,
                         } => {
-                            applogic::denm::handle_denm(etsi);
+                            applogic::denm::handle_denm(&etsi);
 
                             // Send data to BLE thread
                             #[cfg(feature = "ble")]
-                            {
-                                ble::update(ble::BleSendable::Denm(etsi.into()));
+                            critical_section::with(|cs| {
+                                let ble_update_ref = ble::TX_DATA.borrow(cs);
+                                ble_update_ref.replace(Some(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}"),
-                                }
-                            }
+                            // TODO: Send Raw message
                         }
                         #[cfg(feature = "denm")]
                         ItsMessage::DenmV1 {