FIXUP: more detailed printf logging

Cargo.toml

@@ -15,7 +15,7 @@ path = "./src/test.rs"
 required-features = ["std", "cam"]
 
 [features]
-default = ["esp", "spat", "gnss_ubx", "screen"]
+default = ["esp", "spat", "gnss_ubx", "screen", "ble"]
 
 # build for ESP (needed to build test for `host-tuple`)
 esp = [
@@ -57,7 +57,7 @@ screen = ["dep:mipidsi", "dep:embedded-graphics", "dep:embedded-hal-bus", "dep:p
 # enable I/O via UART
 uart = ["_uart", "_gnss"]
 # enable I/O via BLE
-ble = ["_gnss"]
+ble = ["_gnss", "denm"]
 
 # internal feature, that any UART I/O is enabled
 _uart = []

Readme.md

@@ -8,7 +8,7 @@ The code is currently tailored to:
 - U-Blox M10Q GNSS (connected on GPIO12 (RX), GPIO11 (TX), and GPIO25 (reset))  
 or Seeed XIAO L67K (connected on GPIO12 (RX), GPIO11 (TX), GPIO1 (wakeup) and GPIO25 (reset))  
 only available with `gnss_ubx` or `gnss_l67k` feature (which is included in `spat` feature)
-- Some ST7789 240*280px screen (connected on GPIO8 (SCL/ SCK), GPIO10 (SDA/ MOSI), GPIO7 (reset), GPIO23 (DC), GPIO24 (CS), GPIO0 (BL))  
+- Some ST7789 172*320px screen (connected on GPIO8 (SCL/ SCK), GPIO10 (SDA/ MOSI), GPIO7 (reset), GPIO23 (DC), GPIO24 (CS), TODO GPIO0 (BL))  
 only available with `screen` feature (which is included in `spat` feature)
 
 ## Features
@@ -57,10 +57,9 @@ Enable UART I/O protocol according to [docs/uart-protocol.md](./docs/uart-protoc
 
 ### BLE
 Enable BLE I/O protocol according to [docs/ble-protocol.md](./docs/ble-protocol.md).
-Enabling `denm` feature or `cam` feature is recommended, otherwise no data will be received.
+Will enable `denm` feature, but also enabling `cam` is recommended for full functionality.
 
-Note: When changing the set of supported messages on the BLE interface, you may need to turn bluetooth off and on again on your client device.
-Otherwise the client may not re-scan the service's characteristics and still use the previous set of characteristics.
+Note: Raw message reception is currently not implemented!
 
 
 ## Usage

docs/ble-protocol.md

@@ -19,16 +19,12 @@ 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  | `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.
+c0b70001-d4f4-4001-ada8-f99a02ee315c  | `RawMsgRx`      | Raw CAM message (de-duplicated/ rate-limited)
+c0b70001-d4f4-4002-ada8-f99a02ee315c  | `CamEvent`      | Latest received CAM
+c0b70001-d4f4-4003-ada8-f99a02ee315c  | `RawMsgRx`      | Raw DENM message (de-duplicated/ rate-limited)
+c0b70001-d4f4-4004-ada8-f99a02ee315c  | `DenmEvent`     | Latest received DENM
+c0b70001-d4f4-4005-ada8-f99a02ee315c  | `RawMsgRx`      | Raw MAPEM message (de-duplicated/ rate-limited)
+c0b70001-d4f4-4006-ada8-f99a02ee315c  | `RawMsgRx`      | Raw SPATEM message (de-duplicated/ rate-limited)
 
 ## Message Format
 
@@ -105,20 +101,3 @@ 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

docs/c_its-messages.proto

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

feat-permut.sh

@@ -15,8 +15,6 @@ features_base=(
   spat,gnss_ubx,screen,cam_tx
 )
 features_more=(
-  spat,gnss_ubx,screen,ble
-  spat,gnss_ubx,screen,ble,cam_tx
   spat,gnss_ubx,screen,cam,denm
   spat,gnss_ubx,screen,cam_tx,cam,denm
 )

src/applogic/mod.rs

@@ -106,52 +106,7 @@ pub struct GlosaSignalInfo {
 pub struct SignalGroupData {
     pub phase: SignalPhase,
     pub maneuver: map::Maneuvers,
-    pub timing: Option<TimingData>,
-}
-
-#[cfg(feature = "spat")]
-#[derive(Debug, Default, Clone, PartialEq, Eq)]
-#[allow(clippy::struct_field_names)]
-pub struct TimingData {
-    pub min_end_sec: i16,
-    pub likely_end_sec: Option<i16>,
-    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
-            .min_end_time()
-            .map(|val| Self::time_to_duration_sec(now, val));
-        let likely_end_sec = value
-            .likely_time()
-            .map(|val| Self::time_to_duration_sec(now, val));
-        let max_end_sec = value
-            .max_end_time()
-            .map(|val| Self::time_to_duration_sec(now, val));
-
-        min.map(|min_end_sec| Self {
-            min_end_sec,
-            likely_end_sec,
-            max_end_sec,
-        })
-    }
-
-    fn time_to_duration_sec(
-        now: chrono::DateTime<chrono::Utc>,
-        time_point: chrono::DateTime<chrono::Utc>,
-    ) -> i16 {
-        use core::ops::Sub as _;
-
-        use num_traits::Float as _;
-
-        let dur = time_point.sub(now).as_seconds_f32();
-        #[allow(clippy::cast_possible_truncation)]
-        let duration_sec = dur.round() as i16;
-
-        duration_sec
-    }
+    pub end_sec: Option<i16>,
 }
 
 #[cfg(feature = "spat")]
@@ -402,17 +357,33 @@ impl State {
                         let sig_grps = approach_sig_grps
                             .iter()
                             .filter_map(|sig| {
-                                if let Some(maneuver) = sig.maneuvers() {
+                                if let Some(manv) = sig.maneuvers() {
+                                    use core::ops::Sub;
+
+                                    use num_traits::Float;
+
                                     let phase = sig
                                         .phase()
                                         .map(core::convert::Into::into)
                                         .unwrap_or_default();
-                                    let timing = TimingData::new(sig, self.time.and_utc());
+
+                                    let end_sec = sig
+                                        .likely_time()
+                                        .or_else(|| sig.min_end_time())
+                                        .map(|time| {
+                                            let dur =
+                                                time.sub(self.time.and_utc()).as_seconds_f32();
+
+                                            #[allow(clippy::cast_possible_truncation)]
+                                            let end_sec = dur.round() as i16;
+
+                                            end_sec
+                                        });
 
                                     Some(SignalGroupData {
                                         phase,
-                                        maneuver,
-                                        timing,
+                                        maneuver: manv,
+                                        end_sec,
                                     })
                                 } else {
                                     None

src/applogic/v2x/map.rs

@@ -173,12 +173,6 @@ impl Intersection {
                             {
                                 sig_grp.likely_time = Some(likely.to_datetime_from_moy(moy, year));
                             }
-                            if let Some(max) = &timing.max_end_time
-                                && !max.is_out_of_range()
-                                && !max.is_unknown()
-                            {
-                                sig_grp.max_end_time = Some(max.to_datetime_from_moy(moy, year));
-                            }
                         }
                     }
                 } else {
@@ -229,7 +223,6 @@ impl Intersection {
                     target_lanes,
                     phase: None,
                     min_end_time: None,
-                    max_end_time: None,
                     likely_time: None,
                 }
             })
@@ -417,7 +410,7 @@ pub struct SignalGroup {
     /// current signal phase
     phase: Option<etsi_its_dsrc::MovementPhaseState>,
     min_end_time: Option<chrono::DateTime<chrono::Utc>>,
-    max_end_time: Option<chrono::DateTime<chrono::Utc>>,
+    // max_end_time: Option<chrono::DateTime<chrono::Utc>>,
     likely_time: Option<chrono::DateTime<chrono::Utc>>,
 }
 
@@ -450,9 +443,6 @@ impl SignalGroup {
     pub fn likely_time(&self) -> Option<chrono::prelude::DateTime<chrono::prelude::Utc>> {
         self.likely_time
     }
-    pub fn max_end_time(&self) -> Option<chrono::prelude::DateTime<chrono::prelude::Utc>> {
-        self.max_end_time
-    }
 
     pub fn phase_to_str(&self) -> Option<String> {
         self.phase.map(|v| {

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,26 +16,14 @@ 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));
 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]
@@ -44,43 +31,26 @@ 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-4004-ada8-f99a02ee315c", read, notify, value = [0u8; io::msg::CamEvent::PROTO_SIZE])]
-    cam_event: [u8; io::msg::CamEvent::PROTO_SIZE],
+    #[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", 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-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],
+    #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])]
+    denm_event: [u8; DENM_EVENT_SIZE],
 }
 
 /// Creates the BLE controller
@@ -90,10 +60,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(),
@@ -109,7 +75,6 @@ pub async fn run(
         esp_radio::ble::controller::BleConnector<'static>,
         CONTROLLER_SLOTS,
     >,
-    name: alloc::string::String,
 ) {
     let mut resources: HostResources<DefaultPacketPool, CONNECTIONS_MAX, L2CAP_CHANNELS_MAX> =
         HostResources::new();
@@ -120,47 +85,26 @@ pub async fn run(
         ..
     } = stack.build();
 
-    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(",")
-    );
+    info!("Starting advertising and GATT service");
     let server = Server::new_with_config(GapConfig::Peripheral(PeripheralConfig {
-        name: &name,
+        name: "ESP32 C-ITS",
         appearance: &appearance::UNKNOWN,
     }))
     .unwrap();
 
     let _ = embassy_futures::join::join(ble_task(runner), async {
         loop {
-            match advertise(&mut peripheral, &server).await {
+            match advertise("ESP32 C-ITS", &mut peripheral, &server).await {
                 Ok(conn) => {
-                    info!("BLE: New client connected");
-
                     // 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:?}");
+                    panic!("BLE ADV: fatal error {e:?}");
                 }
             }
         }
@@ -168,35 +112,17 @@ 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,
     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 service_uuid = Uuid::from(0xc0b7_0000_d4f4_4000_ada8_f99a_02ee_315c_u128)
-        .as_raw()
-        .try_into()
-        .unwrap();
     let len = AdStructure::encode_slice(
         &[
             AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
-            AdStructure::ServiceUuids128(&[service_uuid]),
+            AdStructure::CompleteLocalName(name.as_bytes()),
         ],
         &mut advertiser_data[..],
     )?;
@@ -275,142 +201,63 @@ async fn gatt_events_task<P: PacketPool>(
     Ok(())
 }
 
-#[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);
-    });
-
+/// 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);
             }
         });
 
         match new_data {
             #[cfg(feature = "cam")]
-            Some(BleSendable::Cam(cam_event)) => match cam_event.serialize_ble_proto() {
-                Ok(value) => {
+            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());
                 }
-                Err(err) => warn!("BLE: CAM event {err}"),
-            },
-
+            }
             #[cfg(feature = "denm")]
-            Some(BleSendable::Denm(denm_event)) => match denm_event.serialize_ble_proto() {
-                Ok(value) => {
+            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());
                 }
-                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:?}"))
-}

src/io.rs

@@ -40,91 +40,20 @@ impl msg::RawMsgRx {
 #[cfg(feature = "denm")]
 impl msg::DenmEvent {
     #[cfg(feature = "ble")]
-    pub const PROTO_SIZE: usize = 51;
-
-    #[cfg(feature = "ble")]
-    pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> {
+    pub fn serialize_ble_proto(&self) -> alloc::vec::Vec<u8> {
         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)
-        }
+        self.encode_to_vec()
     }
 }
 
 #[cfg(feature = "cam")]
 impl msg::CamEvent {
     #[cfg(feature = "ble")]
-    pub const PROTO_SIZE: usize = 38;
-
-    #[cfg(feature = "ble")]
-    pub fn serialize_ble_proto(&self) -> Result<[u8; Self::PROTO_SIZE], alloc::string::String> {
+    pub fn serialize_ble_proto(&self) -> alloc::vec::Vec<u8> {
         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(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)
-        }
+        self.encode_to_vec()
     }
 }
 
@@ -485,11 +414,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;
 
@@ -500,14 +429,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,
@@ -515,7 +442,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()),
@@ -533,8 +460,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 {
@@ -562,11 +489,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;
 
@@ -585,8 +512,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;
@@ -598,7 +524,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());
@@ -667,53 +592,6 @@ 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 {
 

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
 
@@ -101,6 +101,7 @@ fn panic_handler(info: &core::panic::PanicInfo) -> ! {
 #[cfg(feature = "_gnss")]
 static GNSS_UPDATE: Mutex<RefCell<Option<applogic::GnssFix>>> = Mutex::new(RefCell::new(None));
 static WIFI_RX_QUEUE: Mutex<RefCell<Option<ArrayQueue<Vec<u8>>>>> = Mutex::new(RefCell::new(None));
+static mut USER_LED: Option<esp_hal::gpio::Output<'static>> = None;
 #[cfg(feature = "_uart")]
 static UART_RX_BUF: Mutex<RefCell<Option<Vec<u8>>>> = Mutex::new(RefCell::new(None));
 #[cfg(feature = "_uart")]
@@ -154,8 +155,8 @@ async fn main(spawner: Spawner) -> ! {
     let mut spi_buffer = [0_u8; 512];
     #[cfg(feature = "screen")]
     let mut screen = {
-        // 240*280px in portrait orientation
-        let (display, size) = screen::make_large_display(
+        // 172*320px in landscape orientation
+        let (display, width, height) = screen::make_small_display(
             peripherals.GPIO8,
             peripherals.GPIO10,
             peripherals.GPIO7,
@@ -167,7 +168,7 @@ async fn main(spawner: Spawner) -> ! {
         )
         .expect("Fatal error building screen");
 
-        let mut screen = screen::Handler::new(display, size);
+        let mut screen = screen::Handler::new(display, width, height);
 
         let _ = screen
             .update(screen::ScreenState::Initial)
@@ -203,6 +204,13 @@ async fn main(spawner: Spawner) -> ! {
         error!("Failed to set WiFi TX power: {err}");
     }
 
+    unsafe {
+        USER_LED = Some(esp_hal::gpio::Output::new(
+            peripherals.GPIO27,
+            esp_hal::gpio::Level::Low,
+            esp_hal::gpio::OutputConfig::default(),
+        ));
+    }
     critical_section::with(|cs| {
         WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2)));
     });
@@ -219,14 +227,7 @@ async fn main(spawner: Spawner) -> ! {
     #[cfg(feature = "ble")]
     {
         let controller = ble::make_controller(peripherals.BT);
-        let serial = &mac.as_bytes()[3..6];
-        let name = alloc::format!(
-            "ESP32 C-ITS {:02x}{:02x}{:02x}",
-            serial[0],
-            serial[1],
-            serial[2]
-        );
-        spawner.spawn(ble::run(controller, name).expect("Failed to spawn BLE task"));
+        spawner.spawn(ble::run(controller).expect("Failed to spawn BLE task"));
     }
 
     // Setup GNSS:
@@ -379,22 +380,15 @@ async fn main(spawner: Spawner) -> ! {
                                     println!("{}", ubx::pretty_print_nav_sat(packet));
                                 }
                                 ublox::proto27::PacketRef::NavPvt(pvt) => {
-                                    // error can be ignored b/c it's just when no GNSS fix yet
-                                    if let Ok(pos_state) = applogic::GnssFix::try_from(pvt) {
-                                        critical_section::with(|cs| {
-                                            let gnss_update_ref = GNSS_UPDATE.borrow(cs);
-                                            gnss_update_ref.replace(Some(pos_state));
-                                        });
+                                    match applogic::GnssFix::try_from(pvt) {
+                                        Ok(pos_state) => {
+                                            critical_section::with(|cs| {
+                                                let gnss_update_ref = GNSS_UPDATE.borrow(cs);
+                                                gnss_update_ref.replace(Some(pos_state));
+                                            });
+                                        }
+                                        Err(err) => warn!("{err}"),
                                     }
-
-                                    // update GNSS HB on screen
-                                    let _ = screen.update_gnss(pvt);
-                                }
-                                ublox::proto27::PacketRef::AckNak(nak) => {
-                                    error!("UBX: {nak:?}");
-                                }
-                                ublox::proto27::PacketRef::AckAck(ack) => {
-                                    info!("UBX: {ack:?}");
                                 }
                                 _ => {
                                     println!("UBX: {packet_ref:?}");
@@ -474,9 +468,8 @@ async fn main(spawner: Spawner) -> ! {
                     if let applogic::GlosaOutput::Locked(data) = &glosa_data {
                         for sig in &data.signal_groups {
                             let duration_str = sig
-                                .timing
-                                .as_ref()
-                                .map(|v| alloc::format!("for {} s", v.min_end_sec))
+                                .end_sec
+                                .map(|v| alloc::format!("for {v} s"))
                                 .unwrap_or_default();
                             println!("GLOSA: {} {} {duration_str}", sig.maneuver, sig.phase);
                         }
@@ -513,36 +506,16 @@ async fn main(spawner: Spawner) -> ! {
                         ItsMessage::Mapem {
                             geonetworking: _,
                             transport: _,
-                            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}"),
-                            }
-                        }
+                            etsi,
+                        } => state.handle_mapem(&etsi),
                         #[cfg(feature = "spat")]
                         ItsMessage::Spatem {
                             geonetworking: _,
                             transport: _,
-                            ref etsi,
+                            etsi,
                         } => {
                             if state.initialized() {
-                                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}"),
+                                state.handle_spatem(&etsi);
                             }
                         }
 
@@ -550,48 +523,33 @@ 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()));
-
-                                // 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}"),
-                                    }
-                                }
-                            }
+                            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")]
                         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()));
-
-                                match io::msg::RawMsgRx::try_from(&msg) {
-                                    Ok(data) => {
-                                        ble::update(ble::BleSendable::DenmRaw(data));
-                                    }
-                                    Err(err) => warn!("Failed to create RawMsgRx: {err}"),
-                                }
-                            }
+                            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 {
@@ -657,6 +615,12 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) {
     }
 
     // info!("Received frame with {} bytes", frame.len);
+    unsafe {
+        #[allow(static_mut_refs)]
+        if let Some(ref mut led) = USER_LED.as_mut() {
+            led.toggle();
+        }
+    }
 
     // parse GN headers
     match c_its_parser::pcap::remove_wlan_headers(frame.data)
@@ -671,6 +635,7 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) {
         Ok(packet) => {
             // drop hopped (for now)
             if packet.decoded.is_hopped() {
+                esp_println::print!("."); // TODO: debug only!
                 return;
             }
 
@@ -679,9 +644,15 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) {
                 // drop all unsupported message IDs and rate-limit SPATEMs
                 if match message_id {
                     #[cfg(feature = "denm")]
-                    c_its_parser::standards::extensions::ItsMessageId::Denm => false,
+                    c_its_parser::standards::extensions::ItsMessageId::Denm => {
+                        esp_println::print!("D"); // TODO: debug only!
+                        false
+                    }
                     #[cfg(feature = "cam")]
-                    c_its_parser::standards::extensions::ItsMessageId::Cam => false,
+                    c_its_parser::standards::extensions::ItsMessageId::Cam => {
+                        esp_println::print!("C"); // TODO: debug only!
+                        false
+                    }
                     #[cfg(feature = "spat")]
                     c_its_parser::standards::extensions::ItsMessageId::Spatem => {
                         // reduce SPAT rate per station ID
@@ -697,7 +668,10 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) {
                                 i.msg_count = i.msg_count.wrapping_add(1);
 
                                 if (i.msg_count % SPAT_RATE_LIMIT) > 0 {
+                                    esp_println::print!("s"); // TODO: debug only!
                                     drop_msg = true;
+                                } else {
+                                    esp_println::print!("S"); // TODO: debug only!
                                 }
                             });
                         }
@@ -705,8 +679,14 @@ fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) {
                         drop_msg
                     }
                     #[cfg(feature = "spat")]
-                    c_its_parser::standards::extensions::ItsMessageId::Mapem => false,
-                    _ => true,
+                    c_its_parser::standards::extensions::ItsMessageId::Mapem => {
+                        esp_println::print!("M"); // TODO: debug only!
+                        false
+                    }
+                    _ => {
+                        esp_println::print!("?"); // TODO: debug only!
+                        true
+                    }
                 } {
                     return;
                 }
@@ -875,8 +855,6 @@ async fn gnss_ubx_config(reset_pin: esp_hal::peripherals::GPIO25<'static>) {
         cfg_data: &[
             ublox::cfg_val::CfgVal::Uart1OutProtNmea(false),
             ublox::cfg_val::CfgVal::Uart1OutProtUbx(true),
-            // CFG-RATE-MEAS
-            ublox::cfg_val::CfgVal::RateMeas(500),
             // CFG-MSGOUT-UBX_NAV_PVT_UART1
             ublox::cfg_val::CfgVal::MsgOutUbxNavPvtUart1(1),
             // CFG-MSGOUT-UBX_NAV_SAT_UART1

src/screen.rs

@@ -5,24 +5,17 @@ use embedded_graphics::{geometry, mono_font, pixelcolor, primitives, text};
 
 use crate::applogic;
 
-pub struct DisplaySize {
-    width: u16,
-    height: u16,
-    offset_top: i32,
-    corner_radius: i32,
-}
-
 pub struct Handler<D>
 where
     D: DrawTarget<Color = pixelcolor::Rgb565>,
 {
     target: D,
-    size: DisplaySize,
+    width: u16,
+    height: u16,
 
     // internal state
     prev_state: ScreenState,
     hb_state: bool,
-    gnss_hb_state: bool,
 }
 
 #[derive(Debug, Default, PartialEq, Eq)]
@@ -86,20 +79,16 @@ where
 
     const DEFAULT_TEXT_STYLE: mono_font::MonoTextStyle<'static, pixelcolor::Rgb565> =
         mono_font::MonoTextStyle::new(&profont::PROFONT_24_POINT, Self::COLOR_FG);
-    const MID_TEXT_STYLE: mono_font::MonoTextStyle<'static, pixelcolor::Rgb565> =
-        mono_font::MonoTextStyle::new(&profont::PROFONT_18_POINT, Self::COLOR_FG);
     const SMALL_TEXT_STYLE: mono_font::MonoTextStyle<'static, pixelcolor::Rgb565> =
         mono_font::MonoTextStyle::new(&profont::PROFONT_14_POINT, Self::COLOR_FG);
 
-    const BIG_HEIGHT_THLD: u16 = 172;
-
-    pub fn new(target: D, display_size: DisplaySize) -> Self {
+    pub fn new(target: D, width: u16, height: u16) -> Self {
         Self {
             target,
-            size: display_size,
+            width,
+            height,
             prev_state: ScreenState::Uninitialized,
             hb_state: false,
-            gnss_hb_state: false,
         }
     }
 
@@ -126,19 +115,18 @@ where
         // Update indicator/ heartbeat
         {
             let update_ind_dia = 10;
-            let circle_x_pos =
-                i32::from(self.size.width) - update_ind_dia - self.size.corner_radius;
+            let circle_x_pos = i32::from(self.width) - update_ind_dia - 10;
 
             text::Text::with_alignment(
                 "HB",
-                geometry::Point::new(circle_x_pos - 5, 10 + self.size.offset_top),
+                geometry::Point::new(circle_x_pos - 5, 10),
                 Self::SMALL_TEXT_STYLE,
                 text::Alignment::Right,
             )
             .draw(&mut self.target)?;
 
             primitives::Circle::new(
-                geometry::Point::new(circle_x_pos, 2 + self.size.offset_top),
+                geometry::Point::new(circle_x_pos, 2),
                 update_ind_dia.cast_unsigned(),
             )
             .into_styled(if self.hb_state {
@@ -155,67 +143,13 @@ where
         Ok(())
     }
 
-    // on big screns: uses full screen width and height from 250 to 280 pixels
-    pub fn update_gnss(
-        &mut self,
-        pvt: &ublox::nav_pvt::proto27::NavPvtRef,
-    ) -> Result<(), D::Error> {
-        use alloc::string::ToString as _;
-
-        // this only applies to big screens
-        if !self.is_big_height() {
-            return Ok(());
-        }
-
-        let num_sat = pvt.num_satellites();
-        let fix_str = match pvt.fix_type() {
-            ublox::GnssFixType::NoFix
-            | ublox::GnssFixType::DeadReckoningOnly
-            | ublox::GnssFixType::TimeOnlyFix => {
-                alloc::format!("{num_sat:2} sat")
-            }
-            ublox::GnssFixType::Fix2D => "2D fix".to_string(),
-            ublox::GnssFixType::Fix3D | ublox::GnssFixType::GPSPlusDeadReckoning => {
-                "3D fix".to_string()
-            }
-            _ => "     ".to_string(),
-        };
-
-        // clear area and write information
-        // note: text position is x baseline, not top left corner
-        primitives::Rectangle::new(
-            geometry::Point::new(0, 250 + self.size.offset_top),
-            geometry::Size::new(self.size.width.into(), 20),
-        )
-        .into_styled(Self::BK_STYLE)
-        .draw(&mut self.target)?;
-
-        let colon = if self.gnss_hb_state { ":" } else { " " };
-        let message = alloc::format!(
-            "{fix_str}{colon} {:2.0}km/h, {:3.0}°",
-            pvt.ground_speed_2d() * 3.6,
-            pvt.heading_motion()
-        );
-        text::Text::with_alignment(
-            &message,
-            geometry::Point::new(self.size.corner_radius / 2, 265 + self.size.offset_top),
-            Self::SMALL_TEXT_STYLE,
-            text::Alignment::Left,
-        )
-        .draw(&mut self.target)?;
-
-        self.gnss_hb_state = !self.gnss_hb_state;
-
-        Ok(())
-    }
-
     fn clear(&mut self) -> Result<(), D::Error>
     where
         D: DrawTarget<Color = pixelcolor::Rgb565>,
     {
         primitives::Rectangle::new(
             geometry::Point::new(0, 0),
-            geometry::Size::new(self.size.width.into(), self.size.height.into()),
+            geometry::Size::new(self.width.into(), self.height.into()),
         )
         .into_styled(Self::BK_STYLE)
         .draw(&mut self.target)?;
@@ -223,10 +157,6 @@ where
         Ok(())
     }
 
-    fn is_big_height(&self) -> bool {
-        self.size.height > Self::BIG_HEIGHT_THLD
-    }
-
     #[allow(unused)]
     pub fn test_img(&mut self) -> Result<(), D::Error> {
         mipidsi::TestImage::new().draw(&mut self.target)?;
@@ -241,7 +171,7 @@ where
 
         text::Text::with_alignment(
             msg,
-            geometry::Point::new(i32::from(self.size.width) / 2, 40 + self.size.offset_top),
+            geometry::Point::new(i32::from(self.width) / 2, 40),
             Self::DEFAULT_TEXT_STYLE,
             text::Alignment::Center,
         )
@@ -256,8 +186,8 @@ where
         self.clear()?;
 
         text::Text::with_alignment(
-            "Searching...",
-            geometry::Point::new(i32::from(self.size.width) / 2, 130 + self.size.offset_top),
+            "No upcoming SPAT",
+            geometry::Point::new(i32::from(self.width) / 2, 130),
             Self::DEFAULT_TEXT_STYLE,
             text::Alignment::Center,
         )
@@ -279,7 +209,7 @@ where
 
         text::Text::with_alignment(
             &text,
-            geometry::Point::new(i32::from(self.size.width) / 2, 130 + self.size.offset_top),
+            geometry::Point::new(i32::from(self.width) / 2, 130),
             Self::DEFAULT_TEXT_STYLE,
             text::Alignment::Center,
         )
@@ -288,9 +218,6 @@ where
         Ok(())
     }
 
-    // uses full screen width and height
-    // - from 0 to 105+67    = 172 pixels for small screens
-    // - from 0 to 105+67+20 = 192 pixels for big screens
     #[allow(unused)]
     fn draw_glosa(
         &mut self,
@@ -299,8 +226,6 @@ where
     ) -> Result<(), D::Error> {
         let num_signals = data.signal_groups.len();
 
-        let is_big_height = self.is_big_height();
-
         if let Some(prev) = prev_data
             && prev.signal_groups.len() != num_signals
         {
@@ -308,30 +233,25 @@ where
             self.clear();
         } else {
             // only clear maneuver and timing area otherwise
-            let sig_total_height = 67 + if is_big_height { 20 } else { 0 };
+
             primitives::Rectangle::new(
-                geometry::Point::new(0, 105 + self.size.offset_top),
-                geometry::Size::new(self.size.width.into(), sig_total_height),
+                geometry::Point::new(0, 105),
+                geometry::Size::new(self.width.into(), 67),
             )
             .into_styled(Self::BK_STYLE)
             .draw(&mut self.target)?;
         }
 
-        // do nothing more when no signals
-        if num_signals == 0 {
-            return Ok(());
-        }
-
         // draw intersection ID
         text::Text::new(
             &alloc::format!("#{}", data.intersection_id),
-            geometry::Point::new(self.size.corner_radius + 5, 10 + self.size.offset_top),
+            geometry::Point::new(15, 10),
             Self::SMALL_TEXT_STYLE,
         )
         .draw(&mut self.target)?;
 
         #[allow(clippy::cast_possible_truncation, clippy::cast_possible_wrap)]
-        let sig_width = i32::from(self.size.width) / (num_signals as i32);
+        let sig_width = i32::from(self.width) / (num_signals as i32);
 
         for (idx, sig) in data.signal_groups.iter().enumerate() {
             #[allow(clippy::cast_possible_truncation, clippy::cast_possible_wrap)]
@@ -349,7 +269,7 @@ where
                 applogic::SignalPhase::RedYellow | applogic::SignalPhase::Yellow => Self::YE_STYLE,
             };
             primitives::Circle::new(
-                geometry::Point::new(offset + circle_margin, 15 + self.size.offset_top),
+                geometry::Point::new(offset + circle_margin, 15),
                 circle_dia.cast_unsigned(),
             )
             .into_styled(style)
@@ -360,50 +280,24 @@ where
             // draw maneuver
             text::Text::with_alignment(
                 &maneuver_to_str(sig.maneuver),
-                geometry::Point::new(centerline, 130 + self.size.offset_top),
+                geometry::Point::new(centerline, 130),
                 Self::DEFAULT_TEXT_STYLE,
                 text::Alignment::Center,
             )
             .draw(&mut self.target)?;
 
-            // draw timing indicator(s)
-            if let Some(timing) = &sig.timing {
-                if let (Some(likely), Some(max)) = (timing.likely_end_sec, timing.max_end_sec) {
-                    text::Text::with_alignment(
-                        &alloc::format!("{likely}s"),
-                        geometry::Point::new(centerline, 160 + self.size.offset_top),
-                        Self::DEFAULT_TEXT_STYLE,
-                        text::Alignment::Center,
-                    )
-                    .draw(&mut self.target)?;
-
-                    // only for big screen
-                    if is_big_height {
-                        // TODO: show as diff to likely time?
-                        text::Text::with_alignment(
-                            &alloc::format!("{}-{max}", timing.min_end_sec),
-                            geometry::Point::new(centerline, 185 + self.size.offset_top),
-                            Self::MID_TEXT_STYLE,
-                            text::Alignment::Center,
-                        )
-                        .draw(&mut self.target)?;
-                    }
-                } else {
-                    text::Text::with_alignment(
-                        &alloc::format!("{}s", timing.min_end_sec),
-                        geometry::Point::new(centerline, 160 + self.size.offset_top),
-                        Self::DEFAULT_TEXT_STYLE,
-                        text::Alignment::Center,
-                    )
-                    .draw(&mut self.target)?;
-                }
+            // draw timing indicator
+            if let Some(end_sec) = sig.end_sec {
+                text::Text::with_alignment(
+                    &alloc::format!("{end_sec}s"),
+                    geometry::Point::new(centerline, 160),
+                    Self::DEFAULT_TEXT_STYLE,
+                    text::Alignment::Center,
+                )
+                .draw(&mut self.target)?;
             }
         }
 
-        // if is_big_height {
-        //     // TODO: Add ETA
-        // }
-
         Ok(())
     }
 
@@ -413,7 +307,7 @@ where
 
         self.clear()?;
 
-        let sig_width = i32::from(self.size.width) / 3;
+        let sig_width = i32::from(self.width) / 3;
         let circle_dia = 50;
         let circle_margin = (sig_width - circle_dia) / 2;
 
@@ -425,7 +319,7 @@ where
             let offset = (idx as i32) * sig_width;
 
             primitives::Circle::new(
-                geometry::Point::new(offset + circle_margin, 15 + self.size.offset_top),
+                geometry::Point::new(offset + circle_margin, 15),
                 circle_dia.cast_unsigned(),
             )
             .into_styled(*style)
@@ -433,10 +327,10 @@ where
         }
 
         // draw welcome text
-        let centerline = i32::from(self.size.width) / 2;
+        let centerline = i32::from(self.width) / 2;
         text::Text::with_alignment(
-            "C-ITS Signal Phase\nWaiting for GNSS...",
-            geometry::Point::new(centerline, 130 + self.size.offset_top),
+            "C-ITS Signal Phase Display\nWaiting for GNSS fix...",
+            geometry::Point::new(centerline, 130),
             text_style,
             text::Alignment::Center,
         )
@@ -459,7 +353,7 @@ fn maneuver_to_str(value: applogic::v2x::map::Maneuvers) -> alloc::string::Strin
 ///
 /// # Errors
 /// Human-readable fatal errors
-#[allow(unused, clippy::too_many_arguments, clippy::type_complexity)]
+#[allow(clippy::too_many_arguments, clippy::type_complexity)]
 pub fn make_small_display<IoScl, IoSda, IoRes, IoDc, IoCs, IoBl, Spi>(
     scl: IoScl,
     sda: IoSda,
@@ -484,7 +378,8 @@ pub fn make_small_display<IoScl, IoSda, IoRes, IoDc, IoCs, IoBl, Spi>(
             mipidsi::models::ST7789,
             esp_hal::gpio::Output<'_>,
         >,
-        DisplaySize,
+        u16,
+        u16,
     ),
     alloc::string::String,
 >
@@ -501,15 +396,10 @@ where
         esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static,
     Spi: esp_hal::spi::master::Instance + 'static,
 {
+    const DISPLAY_WIDTH: u16 = 320;
+    const DISPLAY_HEIGHT: u16 = 172;
     const DISPLAY_HIDDEN_X: u16 = 34; // somehow this display has 34 px of invisible space on the left (in native orientation)
 
-    let size = DisplaySize {
-        width: 320,
-        height: 172,
-        offset_top: 0,
-        corner_radius: 10,
-    };
-
     let (spi, cs) = setup_st7789_spi(scl, sda, cs, spi)
         .map_err(|err| alloc::format!("Failed to build SPI config: {err}"))?;
 
@@ -523,101 +413,18 @@ where
     let spi_device = embedded_hal_bus::spi::ExclusiveDevice::new_no_delay(spi, cs)
         .map_err(|err| alloc::format!("Failed to initialize SPI device: {err}"))?;
 
-    // Note: height and width are swapped b/c of 90° display rotation
     let display = mipidsi::Builder::new(
         mipidsi::models::ST7789,
         mipidsi::interface::SpiInterface::new(spi_device, dc_output, spi_buffer),
     )
-    .display_size(size.height + DISPLAY_HIDDEN_X, size.width)
+    .display_size(DISPLAY_HEIGHT + DISPLAY_HIDDEN_X, DISPLAY_WIDTH)
     .reset_pin(rst_output)
     .invert_colors(mipidsi::options::ColorInversion::Inverted)
     .orientation(mipidsi::options::Orientation::new().rotate(mipidsi::options::Rotation::Deg90))
     .init(&mut display_delay)
     .map_err(|err| alloc::format!("Failed to initialize display: {err:?}"))?;
 
-    Ok((display, size))
-}
-
-/// Creates an [`mipidsi::Display`], width, height tuple for an ST7789 240*280px screen in portrait orientation
-///
-/// # Errors
-/// Human-readable fatal errors
-#[allow(unused, clippy::too_many_arguments, clippy::type_complexity)]
-pub fn make_large_display<IoScl, IoSda, IoRes, IoDc, IoCs, IoBl, Spi>(
-    scl: IoScl,
-    sda: IoSda,
-    res: IoRes,
-    dc: IoDc,
-    cs: IoCs,
-    bl: IoBl,
-    spi: Spi,
-    spi_buffer: &mut [u8],
-) -> Result<
-    (
-        mipidsi::Display<
-            mipidsi::interface::SpiInterface<
-                '_,
-                embedded_hal_bus::spi::ExclusiveDevice<
-                    esp_hal::spi::master::Spi<'_, esp_hal::Blocking>,
-                    esp_hal::gpio::Output<'_>,
-                    embedded_hal_bus::spi::NoDelay,
-                >,
-                esp_hal::gpio::Output<'_>,
-            >,
-            mipidsi::models::ST7789,
-            esp_hal::gpio::Output<'_>,
-        >,
-        DisplaySize,
-    ),
-    alloc::string::String,
->
-where
-    IoScl: esp_hal::gpio::interconnect::PeripheralOutput<'static>,
-    IoSda: esp_hal::gpio::interconnect::PeripheralOutput<'static>,
-    IoRes:
-        esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static,
-    IoDc:
-        esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static,
-    IoCs:
-        esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static,
-    IoBl:
-        esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static,
-    Spi: esp_hal::spi::master::Instance + 'static,
-{
-    const DISPLAY_HIDDEN_Y: u16 = 20; // somehow this display has 20 px of invisible space on the top (in native orientation)
-
-    let size = DisplaySize {
-        width: 240,
-        height: 280 + DISPLAY_HIDDEN_Y,
-        offset_top: DISPLAY_HIDDEN_Y.into(),
-        corner_radius: 30,
-    };
-
-    let (spi, cs) = setup_st7789_spi(scl, sda, cs, spi)
-        .map_err(|err| alloc::format!("Failed to build SPI config: {err}"))?;
-
-    let output_config = esp_hal::gpio::OutputConfig::default();
-    let dc_output = esp_hal::gpio::Output::new(dc, esp_hal::gpio::Level::Low, output_config);
-    let rst_output = esp_hal::gpio::Output::new(res, esp_hal::gpio::Level::High, output_config);
-    let _ = esp_hal::gpio::Output::new(bl, esp_hal::gpio::Level::High, output_config); // enable backlight
-
-    let mut display_delay = esp_hal::delay::Delay::new();
-
-    let spi_device = embedded_hal_bus::spi::ExclusiveDevice::new_no_delay(spi, cs)
-        .map_err(|err| alloc::format!("Failed to initialize SPI device: {err}"))?;
-
-    let display = mipidsi::Builder::new(
-        mipidsi::models::ST7789,
-        mipidsi::interface::SpiInterface::new(spi_device, dc_output, spi_buffer),
-    )
-    .display_size(size.width, size.height + DISPLAY_HIDDEN_Y)
-    .reset_pin(rst_output)
-    .invert_colors(mipidsi::options::ColorInversion::Inverted)
-    .orientation(mipidsi::options::Orientation::new().rotate(mipidsi::options::Rotation::Deg0))
-    .init(&mut display_delay)
-    .map_err(|err| alloc::format!("Failed to initialize display: {err:?}"))?;
-
-    Ok((display, size))
+    Ok((display, DISPLAY_WIDTH, DISPLAY_HEIGHT))
 }
 
 /// Builds the SPI interface for the ST7789 display