diff --git a/Cargo.lock b/Cargo.lock index 4d9cb80..b0f4a7c 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -624,21 +624,6 @@ dependencies = [ "heapless 0.9.2", ] -[[package]] -name = "embassy_gps" -version = "0.1.0" -source = "git+https://github.com/jbeyerstedt/embassy_gps.git?branch=feat%2Frework-gpsfix#75f1a67a69372234330c0c9618a953c415380b99" -dependencies = [ - "chrono", - "embassy-futures", - "embassy-time", - "embedded-io-async 0.7.0", - "esp-hal", - "heapless 0.9.2", - "log", - "nmea", -] - [[package]] name = "embedded-can" version = "0.4.1" @@ -1194,7 +1179,6 @@ dependencies = [ "embassy-futures", "embassy-sync 0.7.2", "embassy-time", - "embassy_gps", "embedded-graphics", "embedded-hal-bus", "embedded-io 0.7.1", @@ -1782,20 +1766,6 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" -[[package]] -name = "nmea" -version = "0.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2086c773d18da556c05ca235596d163c00379027189bafe2209ae40ebd19717c" -dependencies = [ - "arrayvec", - "cfg-if", - "chrono", - "heapless 0.8.0", - "nom", - "num-traits", -] - [[package]] name = "nom" version = "7.1.3" diff --git a/Cargo.toml b/Cargo.toml index 6dc5d8e..e6fdde1 100644 --- a/Cargo.toml +++ b/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 = [ @@ -40,8 +40,6 @@ std = ["c-its-parser/std", "esp32-cits-core/std"] # enable u-blox GNSS module gnss_ubx = ["_gnss", "_uart", "dep:ublox"] -# enable L67K GNSS module -gnss_l67k = ["_gnss", "dep:embassy_gps"] # echo CAM data on serial cam = ["c-its-parser/cam"] # send CAM autonomously @@ -57,7 +55,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 = [] @@ -105,7 +103,6 @@ esp-radio = { version = "0.18.0", optional = true, features = [ trouble-host = { version = "0.6.0", features = ["gatt", "central", "derive", "default-packet-pool"] } static_cell = "2" -embassy_gps = { version = "0.1.0", optional = true, default-features = false, features = ["esp", "log-04"], git = "https://github.com/jbeyerstedt/embassy_gps.git", branch = "feat/rework-gpsfix" } esp32-cits-core = { version = "0.1.0", default-features = false, git = "https://git.hamburg.ccc.de/jtbx/esp32-cits-core", branch = "main" } embedded-hal-bus = { version = "0.3.0", optional = true } embedded-graphics = { version = "0.8.2", optional = true } diff --git a/Readme.md b/Readme.md index 97dfd2b..6f2c217 100644 --- a/Readme.md +++ b/Readme.md @@ -6,15 +6,14 @@ Displays different C-ITS receive-only use cases for cheap. The code is currently tailored to: - Seeed XIAO ESP32-C5 (8MB PSRAM) - 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) +only available with `gnss_ubx` 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)) only available with `screen` feature (which is included in `spat` feature) ## Features ### GNSS -Enable u-blox or L67K GNSS module and in main application logic/ state. +Enable u-blox GNSS module and in main application logic/ state. ### SPAT Takes the current position and heading from the GNSS receiver and displays the signal phases of the upcoming intersection. @@ -57,10 +56,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 diff --git a/docs/ble-protocol.md b/docs/ble-protocol.md index e8ad492..4560424 100644 --- a/docs/ble-protocol.md +++ b/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 diff --git a/docs/c_its-messages.proto b/docs/c_its-messages.proto index 9323b3e..47e523d 100644 --- a/docs/c_its-messages.proto +++ b/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 // ------------------------------ diff --git a/feat-permut.sh b/feat-permut.sh index 287dbcb..1ef6b62 100755 --- a/feat-permut.sh +++ b/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 ) diff --git a/src/applogic/mod.rs b/src/applogic/mod.rs index 0ced524..55672a7 100644 --- a/src/applogic/mod.rs +++ b/src/applogic/mod.rs @@ -118,7 +118,6 @@ pub struct TimingData { pub max_end_sec: Option, } -#[cfg(feature = "spat")] impl TimingData { pub fn new(value: &map::SignalGroup, now: chrono::DateTime) -> Option { let min = value diff --git a/src/ble.rs b/src/ble.rs index bdb32be..b585e86 100644 --- a/src/ble.rs +++ b/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> = Mutex::new(RefCell::new(false)); -pub static TX_DATA: Mutex>>> = - Mutex::new(RefCell::new(None)); +pub static TX_DATA: Mutex>> = Mutex::new(RefCell::new(None)); pub static RX_DATA: Mutex>> = 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 = 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, BleHostError> { 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( Ok(()) } -#[allow(clippy::too_many_lines)] -async fn publishing_task(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(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( - 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, ¬ify_buf) - .await - .map_err(|err| alloc::format!("failed notifying connection of new raw {msg_id:?}: {err:?}")) -} diff --git a/src/io.rs b/src/io.rs index 642807f..3ef8c6d 100644 --- a/src/io.rs +++ b/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 { 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 { 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 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> +impl From> for msg::CamEvent { fn from( - value: &alloc::boxed::Box, + value: alloc::boxed::Box, ) -> Self { use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions; @@ -500,14 +429,12 @@ impl From<&alloc::boxed::Box 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 for msg::VehicleContainer { - fn from(value: &c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency) -> Self { +impl From 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> +impl From> for msg::DenmEvent { fn from( - value: &alloc::boxed::Box, + value: alloc::boxed::Box, ) -> Self { let denm_mgmt = &value.denm.management; @@ -585,8 +512,7 @@ impl From<&alloc::boxed::Box 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 { - 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 { diff --git a/src/main.rs b/src/main.rs index ae2a14e..bc45fcc 100644 --- a/src/main.rs +++ b/src/main.rs @@ -8,16 +8,11 @@ // #![deny(clippy::large_stack_frames)] // we can't use UART I/O and GNSS at the same time -#[cfg(all(feature = "uart", feature = "gnss_ubx", feature = "gnss_l67k"))] -compile_error!( - "feature \"uart\" and features \"gnss_ubx\" or \"gnss_l67k\" cannot be enabled at the same time" -); +#[cfg(all(feature = "uart", feature = "gnss_ubx"))] +compile_error!("feature \"uart\" and feature \"gnss_ubx\" cannot be enabled at the same time"); // autonomous CAM generation need a time and position source (aka _gnss) #[cfg(all(feature = "cam_tx", not(feature = "_gnss")))] compile_error!("feature \"cam_tx\" needs a time and position source (either GNSS or UART)"); -// 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"); @@ -34,8 +29,6 @@ use c_its_parser::standards::extensions::ItsStationType; use critical_section::Mutex; use crossbeam_queue::ArrayQueue; use embassy_executor::Spawner; -#[cfg(feature = "gnss_l67k")] -use embassy_gps::gps::l76k; use esp_backtrace as _; use esp_hal::clock::CpuClock; use esp_hal::timer::timg::TimerGroup; @@ -68,9 +61,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 @@ -219,34 +212,9 @@ 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: - // Seeed XIAO L67K: - // RX: D7/ GPIO12 - // TX: D6/ GPIO11 - // WAKEUP: D0/ GPIO1 -> HIGH for active, LOW for Sleep - // RESET: D2/ GPIO25 -> HIGH for normal, LOW for Reset - #[cfg(feature = "gnss_l67k")] - spawner.spawn( - gnss_l67k_task( - peripherals.GPIO1, - peripherals.GPIO25, - peripherals.UART0, - peripherals.GPIO12, - peripherals.GPIO11, - ) - .expect("Failed to spawn GNSS task"), - ); - // U-Blox module: // RX: D7/ GPIO12 // TX: D6/ GPIO11 @@ -379,12 +347,14 @@ 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 @@ -513,36 +483,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 +500,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 { @@ -800,59 +735,6 @@ fn get_its_header<'p>( } } -#[cfg(feature = "gnss_l67k")] -#[allow(clippy::large_stack_frames, reason = "GNSS driver needs some space")] -#[embassy_executor::task] -async fn gnss_l67k_task( - gps_wakeup: esp_hal::peripherals::GPIO1<'static>, - gps_reset: esp_hal::peripherals::GPIO25<'static>, - uart: esp_hal::peripherals::UART0<'static>, - uart_rx: esp_hal::peripherals::GPIO12<'static>, - uart_tx: esp_hal::peripherals::GPIO11<'static>, -) { - use embassy_gps::gps::GpsFsm; - - let mut gps = l76k::esp::L76kFsm::new_sep( - l76k::esp::GpsHw { - reinit: gps_reset, - standby: gps_wakeup, - }, - || { - esp_hal::uart::Uart::new(uart, esp_hal::uart::Config::default().with_baudrate(9600)) - .expect("Failed to create UART for GNSS") - .with_rx(uart_rx) - .with_tx(uart_tx) - .into_async() - }, - ) - .await; - - let mut fix_drop_message = false; - loop { - if let Ok(Some(embassy_gps::types::GpsEvent::Fix(fix))) = gps.step().await { - // drop every second message as we always get each fix twice - fix_drop_message = !fix_drop_message; - if fix_drop_message { - // send to main thread - let pos_state = applogic::GnssFix { - time: fix.get_timestamp(), - latitude_deg: fix.latitude_deg, - longitude_deg: fix.longitude_deg, - heading_deg: fix.true_course_deg, - speed_mps: fix.speed_over_ground, - }; - - critical_section::with(|cs| { - let gnss_update_ref = GNSS_UPDATE.borrow(cs); - gnss_update_ref.replace(Some(pos_state)); - }); - } - } else { - // FSM will recover automatically from errors and ignore other event types - } - } -} - #[cfg(feature = "gnss_ubx")] #[embassy_executor::task] async fn gnss_ubx_config(reset_pin: esp_hal::peripherals::GPIO25<'static>) {