Add basic BLE I/O

src/applogic/mod.rs

@@ -263,6 +263,7 @@ impl State {
     }
 
     #[cfg(feature = "spat")]
+    #[allow(clippy::too_many_lines)]
     pub fn run_glosa(&mut self) -> GlosaOutput {
         let Some(own_heading) = self.pos.heading_deg else {
             use alloc::string::ToString;

src/ble.rs

@@ -0,0 +1,263 @@
+#![allow(
+    clippy::needless_borrows_for_generic_args,
+    reason = "gatt_service creates false positives"
+)]
+
+use core::cell::RefCell;
+
+use critical_section::Mutex;
+use esp_backtrace as _;
+use log::{info, warn};
+use trouble_host::prelude::*;
+
+use crate::io;
+
+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 RX_DATA: Mutex<RefCell<Option<io::msg::PositionState>>> = Mutex::new(RefCell::new(None));
+
+pub enum BleSendable {
+    #[cfg(feature = "cam")]
+    Cam(io::msg::CamEvent),
+    #[cfg(feature = "denm")]
+    Denm(io::msg::DenmEvent),
+}
+
+#[gatt_server]
+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_Event", read, value = "CamEvent proto msg", type = &'static str)]
+    #[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])]
+    cam_event: [u8; CAM_EVENT_SIZE],
+
+    #[cfg(feature = "denm")]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM_Event", read, value = "DenmEvent proto msg", type = &'static str)]
+    #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])]
+    denm_event: [u8; DENM_EVENT_SIZE],
+}
+
+/// Creates the BLE controller
+pub fn make_controller(
+    bt_peripheral: esp_hal::peripherals::BT<'static>,
+) -> bt_hci::controller::ExternalController<
+    esp_radio::ble::controller::BleConnector<'static>,
+    CONTROLLER_SLOTS,
+> {
+    let transport = esp_radio::ble::controller::BleConnector::new(
+        bt_peripheral,
+        esp_radio::ble::Config::default(),
+    )
+    .unwrap();
+    bt_hci::controller::ExternalController::<_, { CONTROLLER_SLOTS }>::new(transport)
+}
+
+/// Run the BLE stack.
+#[embassy_executor::task]
+pub async fn run(
+    controller: bt_hci::controller::ExternalController<
+        esp_radio::ble::controller::BleConnector<'static>,
+        CONTROLLER_SLOTS,
+    >,
+) {
+    let mut resources: HostResources<DefaultPacketPool, CONNECTIONS_MAX, L2CAP_CHANNELS_MAX> =
+        HostResources::new();
+    let stack = trouble_host::new(controller, &mut resources);
+    let Host {
+        mut peripheral,
+        runner,
+        ..
+    } = stack.build();
+
+    info!("Starting advertising and GATT service");
+    let server = Server::new_with_config(GapConfig::Peripheral(PeripheralConfig {
+        name: "ESP32 C-ITS",
+        appearance: &appearance::UNKNOWN,
+    }))
+    .unwrap();
+
+    let _ = embassy_futures::join::join(ble_task(runner), async {
+        loop {
+            match advertise("ESP32 C-ITS", &mut peripheral, &server).await {
+                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.
+                    embassy_futures::select::select(a, b).await;
+                }
+                Err(e) => {
+                    panic!("BLE ADV: fatal error {e:?}");
+                }
+            }
+        }
+    })
+    .await;
+}
+
+/// 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,
+    peripheral: &mut Peripheral<'values, C, DefaultPacketPool>,
+    server: &'server Server<'values>,
+) -> Result<GattConnection<'values, 'server, DefaultPacketPool>, BleHostError<C::Error>> {
+    let mut advertiser_data = [0; 35];
+    let len = AdStructure::encode_slice(
+        &[
+            AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
+            AdStructure::CompleteLocalName(name.as_bytes()),
+        ],
+        &mut advertiser_data[..],
+    )?;
+
+    let advertiser = peripheral
+        .advertise(
+            &trouble_host::advertise::AdvertisementParameters {
+                timeout: Some(embassy_time::Duration::from_secs(2)),
+                ..Default::default()
+            },
+            Advertisement::ConnectableScannableUndirected {
+                adv_data: &advertiser_data[..len],
+                scan_data: &[],
+            },
+        )
+        .await?;
+    // info!("[adv] advertising {len} bytes");
+    let conn = advertiser.accept().await?.with_attribute_server(server)?;
+    // info!("[adv] connection established");
+    Ok(conn)
+}
+
+async fn ble_task<C: Controller, P: PacketPool>(mut runner: Runner<'_, C, P>) {
+    loop {
+        if let Err(e) = runner.run().await {
+            panic!("BLE Task: fatal error {e:?}");
+        }
+    }
+}
+
+/// Stream Events until the connection closes.
+///
+/// This function will handle the GATT events and process them.
+/// This is how we interact with read and write requests.
+async fn gatt_events_task<P: PacketPool>(
+    server: &Server<'_>,
+    conn: &GattConnection<'_, '_, P>,
+) -> Result<(), Error> {
+    let reason = loop {
+        match conn.next().await {
+            GattConnectionEvent::Disconnected { reason } => break reason,
+            GattConnectionEvent::Gatt { event } => {
+                match &event {
+                    GattEvent::Write(event)
+                        if event.handle() == server.c_its_events.position.handle =>
+                    {
+                        use prost::Message as _;
+
+                        // parse and send to main thread
+                        match io::msg::PositionState::decode(event.data()) {
+                            Ok(data) => {
+                                critical_section::with(|cs| {
+                                    let rx_data_ref = RX_DATA.borrow(cs);
+                                    let _ = rx_data_ref.replace(Some(data));
+                                });
+                            }
+                            Err(err) => log::error!("BLE: Failed to parse input: {err:?}"),
+                        }
+                    }
+
+                    _ => {}
+                }
+
+                // This step is also performed at drop(), but writing it explicitly is necessary
+                // in order to ensure reply is sent.
+                match event.accept() {
+                    Ok(reply) => reply.send().await,
+                    Err(e) => warn!("BLE: error sending response: {e:?}"),
+                }
+            }
+            _ => {} // ignore other Gatt Connection Events
+        }
+    };
+
+    info!("BLE: disconnected b/c {reason:?}");
+    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>) {
+    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);
+
+            if let Some(tx_data) = tx_data_ref.replace(None) {
+                new_data = Some(tx_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();
+
+                    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());
+                }
+            }
+            #[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();
+
+                    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());
+                }
+            }
+            None => {}
+        }
+    }
+}

src/io.rs

@@ -1,16 +1,14 @@
-//! UART/ BLE I/O Protocol
+//! UART and BLE I/O Protocol
 
 pub mod msg {
     #![allow(clippy::all, clippy::pedantic, clippy::nursery, dead_code)]
     include!(concat!(env!("OUT_DIR"), "/c_its_io.rs"));
 }
 
-use alloc::string::ToString;
-use alloc::vec::Vec;
-
+#[cfg(feature = "uart")]
 use c_its_parser::gn as geonetworking;
-use log::error;
 
+#[cfg(feature = "uart")]
 impl From<msg::RawMsgRx> for msg::SerialOutputMsg {
     fn from(value: msg::RawMsgRx) -> Self {
         use msg::serial_output_msg::Payload;
@@ -21,6 +19,7 @@ impl From<msg::RawMsgRx> for msg::SerialOutputMsg {
     }
 }
 
+#[cfg(feature = "uart")]
 impl From<msg::DenmEvent> for msg::SerialOutputMsg {
     fn from(value: msg::DenmEvent) -> Self {
         use msg::serial_output_msg::Payload;
@@ -32,22 +31,39 @@ impl From<msg::DenmEvent> for msg::SerialOutputMsg {
 }
 
 impl msg::RawMsgRx {
-    pub fn serialize_uart_proto(&self) -> Vec<u8> {
+    #[cfg(feature = "uart")]
+    pub fn serialize_uart_proto(&self) -> alloc::vec::Vec<u8> {
         serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(self.clone()))
     }
 }
+
+#[cfg(feature = "denm")]
 impl msg::DenmEvent {
-    #[allow(unused)]
-    pub fn serialize_uart_proto(&self) -> Vec<u8> {
-        serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(*self))
+    #[cfg(feature = "ble")]
+    pub fn serialize_ble_proto(&self) -> alloc::vec::Vec<u8> {
+        use prost::Message as _;
+
+        self.encode_to_vec()
     }
 }
 
+#[cfg(feature = "cam")]
+impl msg::CamEvent {
+    #[cfg(feature = "ble")]
+    pub fn serialize_ble_proto(&self) -> alloc::vec::Vec<u8> {
+        use prost::Message as _;
+
+        self.encode_to_vec()
+    }
+}
+
+#[cfg(feature = "uart")]
 #[derive(Debug, Default)]
 pub struct Parser {
     buffer: alloc::vec::Vec<u8>,
 }
 
+#[cfg(feature = "uart")]
 impl Parser {
     const SOF_LEN: u16 = 4;
     const SOF1: u8 = 0x56;
@@ -91,7 +107,7 @@ impl Parser {
                         Ok(data) => {
                             output.push(data);
                         }
-                        Err(err) => error!("Failed to parse input: {err:?}"),
+                        Err(err) => log::error!("Failed to parse input: {err:?}"),
                     }
 
                     // consume buffer
@@ -122,7 +138,8 @@ impl Parser {
     }
 }
 
-fn serialize_serial_msg<T>(msg: &T) -> Vec<u8>
+#[cfg(feature = "uart")]
+fn serialize_serial_msg<T>(msg: &T) -> alloc::vec::Vec<u8>
 where
     T: prost::Message,
 {
@@ -141,6 +158,7 @@ where
     out
 }
 
+#[cfg(any(feature = "uart", feature = "ble"))]
 impl From<msg::PositionState> for crate::applogic::GnssFix {
     fn from(value: msg::PositionState) -> Self {
         let time =
@@ -169,6 +187,7 @@ impl From<msg::PositionState> for crate::applogic::GnssFix {
     }
 }
 
+#[cfg(feature = "uart")]
 impl From<msg::PositionState> for esp32_cits_core::PosVel {
     fn from(value: msg::PositionState) -> Self {
         let position = geo_types::Point::new(
@@ -194,6 +213,7 @@ impl From<msg::PositionState> for esp32_cits_core::PosVel {
     }
 }
 
+#[cfg(feature = "uart")]
 impl From<msg::ItsPosition> for esp32_cits_core::PosVel {
     fn from(value: msg::ItsPosition) -> Self {
         let latitude_deg = f64::from(value.latitude) / 10_000_000.;
@@ -208,6 +228,7 @@ impl From<msg::ItsPosition> for esp32_cits_core::PosVel {
     }
 }
 
+#[cfg(feature = "uart")]
 impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx {
     fn make_eh(
         &self,
@@ -223,6 +244,8 @@ impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx {
         ),
         alloc::string::String,
     > {
+        use alloc::string::ToString as _;
+
         let station_type = self
             .station_type
             .and_then(|v| {
@@ -296,14 +319,20 @@ impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx {
 
     fn make_payload(
         &self,
-    ) -> Result<(Vec<u8>, c_its_parser::standards::extensions::ItsMessageId), alloc::string::String>
-    {
+    ) -> Result<
+        (
+            alloc::vec::Vec<u8>,
+            c_its_parser::standards::extensions::ItsMessageId,
+        ),
+        alloc::string::String,
+    > {
         #[allow(clippy::cast_possible_truncation)]
         let message_id = (self.message_type as u8).try_into()?;
         Ok((self.payload.clone(), message_id))
     }
 }
 
+#[cfg(feature = "uart")]
 impl msg::RawMsgTx {
     fn make_geobcast(
         address: [u8; 6],
@@ -384,12 +413,192 @@ impl msg::RawMsgTx {
     }
 }
 
+#[cfg(feature = "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>,
+    ) -> Self {
+        use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions;
+
+        let station_id = value.header.station_id.0;
+        let station_type = u32::from(value.cam.cam_parameters.basic_container.station_type.0);
+        let position = value
+            .cam
+            .cam_parameters
+            .basic_container
+            .reference_position
+            .into();
+
+        let vehicle_role = value
+            .cam
+            .cam_parameters
+            .low_frequency_container
+            .and_then(|v| match v {
+                cam_pdu_descriptions::LowFrequencyContainer::basicVehicleContainerLowFrequency(
+                    basic_vehicle_container_low_frequency,
+                ) => Some(basic_vehicle_container_low_frequency.vehicle_role as u32),
+                _ => None,
+            });
+
+        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()),
+            _ => None,
+        };
+
+        Self {
+            station_id,
+            station_type,
+            position,
+            vehicle_role,
+            vehicle_data,
+        }
+    }
+}
+
+#[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 {
+        let heading_deg = if value.heading.heading_value.is_unavailable() {
+            None
+        } else {
+            Some(u32::from(value.heading.heading_value.0))
+        };
+        let speed = if value.speed.speed_value.is_unavailable() {
+            None
+        } else {
+            Some(u32::from(value.speed.speed_value.0))
+        };
+        let vehicle_length_dm = if value.vehicle_length.vehicle_length_value.is_unavailable() {
+            None
+        } else {
+            Some(u32::from(value.vehicle_length.vehicle_length_value.0))
+        };
+        let vehicle_width_dm = if value.vehicle_width.is_unavailable() {
+            None
+        } else {
+            Some(u32::from(value.vehicle_width.0))
+        };
+
+
+        Self { heading_deg, speed, vehicle_length_dm, vehicle_width_dm }
+    }
+}
+
+#[cfg(feature = "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>,
+    ) -> Self {
+        let denm_mgmt = &value.denm.management;
+
+        let station_id = value.header.station_id.0; // TODO: or from action ID?
+
+        let ref_time = chrono::DateTime::<chrono::Utc>::from(denm_mgmt.reference_time.clone());
+        let end_time = ref_time + chrono::Duration::seconds(denm_mgmt.validity_duration.0.into());
+
+        #[allow(clippy::cast_sign_loss)]
+        let timestamp = ref_time.timestamp_millis() as u64;
+        #[allow(clippy::cast_sign_loss)]
+        let validity_end_ts = end_time.timestamp_millis() as u64;
+        let seq_num = denm_mgmt.action_id.sequence_number.0.into();
+
+        let event_position = Some(denm_mgmt.event_position.clone().into());
+        let event_heading_deg = value
+            .denm
+            .location
+            .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;
+                conv
+            });
+        let direction = denm_mgmt
+            .traffic_direction
+            .map(|v| msg::TrafficDir::from(v) as i32);
+        let cc_tuple = value
+            .denm
+            .situation
+            .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());
+
+        Self {
+            timestamp,
+            validity_end_ts,
+            station_id,
+            seq_num,
+            event_position,
+            event_heading_deg,
+            direction,
+            cause_code,
+            sub_cause_code,
+        }
+    }
+}
+
+#[cfg(feature = "cam")]
+impl From<c_its_parser::standards::cdd_1_3_1_1::its_container::ReferencePosition>
+    for msg::Position
+{
+    fn from(value: c_its_parser::standards::cdd_1_3_1_1::its_container::ReferencePosition) -> Self {
+        #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
+        let latitude_deg = value.latitude.as_deg() as f32;
+        #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
+        let longitude_deg = value.longitude.as_deg() as f32;
+
+        Self {
+            latitude_deg,
+            longitude_deg,
+        }
+    }
+}
+
+#[cfg(feature = "denm")]
+impl From<c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::ReferencePosition> for msg::Position {
+    fn from(value: c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::ReferencePosition) -> Self {
+        #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
+        let latitude_deg = value.latitude.as_deg() as f32;
+        #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
+        let longitude_deg = value.longitude.as_deg() as f32;
+
+        Self {
+            latitude_deg,
+            longitude_deg,
+        }
+    }
+}
+
+#[cfg(feature = "denm")]
+impl From<c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection> for msg::TrafficDir {
+    fn from(value: c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection) -> Self {
+        use c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection;
+
+        match value {
+            TrafficDirection::allTrafficDirections => Self::AllDirections,
+            TrafficDirection::sameAsReferenceDirection_upstreamOfReferencePosition => {
+                Self::Upstream
+            }
+            TrafficDirection::sameAsReferenceDirection_downstreamOfReferencePosition => {
+                Self::Downstream
+            }
+            TrafficDirection::oppositeToReferenceDirection => Self::Opposite,
+        }
+    }
+}
+
 #[cfg(test)]
 mod tests {
 
     use super::*;
     use crate::init_test_env_logger;
 
+    #[cfg(feature = "uart")]
     impl From<msg::RawMsgTx> for msg::SerialInputMsg {
         fn from(value: msg::RawMsgTx) -> Self {
             use msg::serial_input_msg::Payload;
@@ -400,6 +609,7 @@ mod tests {
         }
     }
 
+    #[cfg(feature = "uart")]
     impl From<msg::PositionState> for msg::SerialInputMsg {
         fn from(value: msg::PositionState) -> Self {
             use msg::serial_input_msg::Payload;
@@ -413,7 +623,9 @@ mod tests {
     // manual example data:
     // SerialInputMsg(RawMsgTx): 5632582b0040123e0a340202c173256817284029d5b3400f1feaf01ffffffc23b7743e00000fc0007e8138250737feebfff600000dffff7ffff1ce40400010011807220028020d0a
     // SerialInputMsg(PositionState): 5632582b00170a1509a0bbe2a29f010000120a0d00002842150000b8410d0a
+    // PositionState: 09a0bbe2a29f010000120a0d00002842150000b841
 
+    #[cfg(feature = "uart")]
     #[test]
     fn uart_parser() {
         // generate some test UART input messages

src/main.rs

@@ -18,6 +18,9 @@ compile_error!("feature \"cam_tx\" needs a time and position source (either GNSS
 // we can't have both GNSS drivers enabled at the same time
 #[cfg(all(feature = "gnss_ubx", feature = "gnss_l67k"))]
 compile_error!("feature \"gnss_ubx\" and feature \"gnss_l67k\" cannot be enabled at the same time");
+// at least one V2X feature needs to be enabled
+#[cfg(not(any(feature = "spat", feature = "cam", feature = "denm")))]
+compile_error!("at least one V2X feature (\"spat\", \"cam\" or \"denm\") needs to be enabled");
 
 use alloc::string::ToString as _;
 use alloc::vec::Vec;
@@ -45,9 +48,11 @@ use log::{error, info, warn};
 extern crate alloc;
 
 mod applogic;
+#[cfg(feature = "ble")]
+mod ble;
 mod cache;
 mod geo_alg;
-#[cfg(feature = "uart")]
+#[cfg(any(feature = "uart", feature = "ble"))]
 mod io;
 #[cfg(feature = "screen")]
 mod screen;
@@ -89,7 +94,7 @@ static SERIAL: Mutex<RefCell<Option<esp_hal::uart::Uart<'static, esp_hal::Blocki
 #[esp_rtos::main]
 async fn main(spawner: Spawner) -> ! {
     // generator version: 1.3.0
-    // generator parameters: --chip esp32c5 -o esp32c5-wroom-1-psram -o alloc -o log -o unstable-hal -o wifi -o esp-backtrace -o embassy
+    // generator parameters: --chip esp32c5 -o esp32c5-wroom-1-psram -o alloc -o log -o unstable-hal -o wifi -o esp-backtrace -o embassy -o ble-trouble
 
     esp_println::logger::init_logger_from_env();
 
@@ -107,6 +112,8 @@ async fn main(spawner: Spawner) -> ! {
     // - GPIO28
 
     esp_alloc::heap_allocator!(#[esp_hal::ram(reclaimed)] size: 65536);
+    // COEX needs more RAM - so we've added some more
+    esp_alloc::heap_allocator!(size: 64 * 1024);
     esp_alloc::psram_allocator!(peripherals.PSRAM, esp_hal::psram);
 
     let timg0 = TimerGroup::new(peripherals.TIMG0);
@@ -188,6 +195,13 @@ async fn main(spawner: Spawner) -> ! {
     let mac = esp_hal::efuse::base_mac_address();
     info!("WiFi MAC: {mac}");
 
+    // Setup BLE
+    #[cfg(feature = "ble")]
+    {
+        let controller = ble::make_controller(peripherals.BT);
+        spawner.spawn(ble::run(controller).expect("Failed to spawn BLE task"));
+    }
+
     // Setup GNSS:
     // Seeed XIAO L67K:
     // RX:      D7/ GPIO12
@@ -365,6 +379,17 @@ async fn main(spawner: Spawner) -> ! {
             }
         }
 
+        #[cfg(feature = "ble")]
+        {
+            critical_section::with(|cs| {
+                let mut data_rc = ble::RX_DATA.borrow(cs).borrow_mut();
+                if let Some(pos) = data_rc.take() {
+                    let gnss_update_ref = GNSS_UPDATE.borrow(cs);
+                    gnss_update_ref.replace(Some(pos.into()));
+                }
+            });
+        }
+
         #[cfg(feature = "_gnss")]
         {
             let mut new_position = false;
@@ -474,6 +499,13 @@ async fn main(spawner: Spawner) -> ! {
                         } => {
                             #[cfg(feature = "cam")]
                             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())));
+                            });
                         }
 
                         #[cfg(feature = "denm")]
@@ -481,7 +513,16 @@ async fn main(spawner: Spawner) -> ! {
                             geonetworking: _,
                             transport: _,
                             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())));
+                            });
+                        }
                         #[cfg(feature = "denm")]
                         ItsMessage::DenmV1 {
                             geonetworking: _,