From fd8f28ef33927c7b2725944d608770b34e7f1989 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Mon, 11 May 2026 21:08:23 +0200 Subject: [PATCH] WIP: Enable BLE TODO: Receive from BLE --- Cargo.lock | 183 +++++++++++++++++++++++++++ Cargo.toml | 14 ++- Readme.md | 2 +- docs/ble-protocol.md | 50 ++++++++ docs/c_its-messages.proto | 2 +- src/ble.rs | 256 ++++++++++++++++++++++++++++++++++++++ src/io.rs | 213 ++++++++++++++++++++++++++++++- src/main.rs | 31 ++++- 8 files changed, 741 insertions(+), 10 deletions(-) create mode 100644 src/ble.rs diff --git a/Cargo.lock b/Cargo.lock index 98a4c6a..4d9cb80 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -166,6 +166,29 @@ dependencies = [ "generic-array", ] +[[package]] +name = "bt-hci" +version = "0.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "211713d2e9fb4793ce4360a712c0764264aff6be48932ccf02ca2a331c0436a9" +dependencies = [ + "btuuid", + "embassy-sync 0.8.0", + "embedded-io 0.7.1", + "embedded-io-async 0.7.0", + "futures-intrusive", + "heapless 0.9.2", +] + +[[package]] +name = "btuuid" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f5f48f1e9b0aad0a4f05d17bdeae0fa20ff798e272a03a6940ca27ad9c5a6ae7" +dependencies = [ + "uuid", +] + [[package]] name = "bumpalo" version = "3.20.2" @@ -244,6 +267,15 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0b396d1f76d455557e1218ec8066ae14bba60b4b36ecd55577ba979f5db7ecaa" +[[package]] +name = "convert_case" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baaaa0ecca5b51987b9423ccdc971514dd8b0bb7b4060b983d3664dad3f1f89f" +dependencies = [ + "unicode-segmentation", +] + [[package]] name = "cordyceps" version = "0.3.4" @@ -956,6 +988,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "23fbff98b06a96b6ce3791ecec5c668524052a068e23aacd23afe17ddba844ce" dependencies = [ "allocator-api2", + "bt-hci", "cfg-if", "docsplay", "document-features", @@ -1152,11 +1185,14 @@ name = "esp32-c_its-companion" version = "0.1.0" dependencies = [ "assert_float_eq", + "bt-hci", "c-its-parser", "chrono", "critical-section", "crossbeam-queue", "embassy-executor", + "embassy-futures", + "embassy-sync 0.7.2", "embassy-time", "embassy_gps", "embedded-graphics", @@ -1179,6 +1215,8 @@ dependencies = [ "profont", "prost", "prost-build", + "static_cell", + "trouble-host", "ublox", ] @@ -1320,12 +1358,52 @@ version = "2.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e6d5a32815ae3f33302d95fdcb2ce17862f8c65363dcfd29360480ba1001fc9c" +[[package]] +name = "futures" +version = "0.3.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b147ee9d1f6d097cef9ce628cd2ee62288d963e16fb287bd9286455b241382d" +dependencies = [ + "futures-channel", + "futures-core", + "futures-io", + "futures-sink", + "futures-task", + "futures-util", +] + +[[package]] +name = "futures-channel" +version = "0.3.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "07bbe89c50d7a535e539b8c17bc0b49bdb77747034daa8087407d655f3f7cc1d" +dependencies = [ + "futures-core", + "futures-sink", +] + [[package]] name = "futures-core" version = "0.3.32" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7e3450815272ef58cec6d564423f6e755e25379b217b0bc688e295ba24df6b1d" +[[package]] +name = "futures-intrusive" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d930c203dd0b6ff06e0201a4a2fe9149b43c684fd4420555b26d21b1a02956f" +dependencies = [ + "futures-core", + "lock_api", +] + +[[package]] +name = "futures-io" +version = "0.3.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cecba35d7ad927e23624b22ad55235f2239cfa44fd10428eecbeba6d6a717718" + [[package]] name = "futures-sink" version = "0.3.32" @@ -1345,8 +1423,10 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "389ca41296e6190b48053de0321d02a77f32f8a5d2461dd38762c0593805c6d6" dependencies = [ "futures-core", + "futures-sink", "futures-task", "pin-project-lite", + "slab", ] [[package]] @@ -1561,6 +1641,18 @@ dependencies = [ "syn 2.0.117", ] +[[package]] +name = "js-sys" +version = "0.3.98" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67df7112613f8bfd9150013a0314e196f4800d3201ae742489d999db2f979f08" +dependencies = [ + "cfg-if", + "futures-util", + "once_cell", + "wasm-bindgen", +] + [[package]] name = "lazy_static" version = "1.5.0" @@ -1603,6 +1695,15 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "11d3d7f243d5c5a8b9bb5d6dd2b1602c0cb0b9db1621bafc7ed66e35ff9fe092" +[[package]] +name = "lock_api" +version = "0.4.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "224399e74b87b5f3557511d98dff8b14089b3dadafcab6bb93eab67d3aace965" +dependencies = [ + "scopeguard", +] + [[package]] name = "log" version = "0.4.29" @@ -2184,6 +2285,12 @@ version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e1cf6437eb19a8f4a6cc0f7dca544973b0b78843adbfeb3683d1a94a0024a294" +[[package]] +name = "scopeguard" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" + [[package]] name = "semver" version = "1.0.28" @@ -2261,6 +2368,12 @@ version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" +[[package]] +name = "slab" +version = "0.4.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c790de23124f9ab44544d7ac05d60440adc586479ce501c1d6d7da3cd8c9cf5" + [[package]] name = "smallvec" version = "1.15.1" @@ -2309,6 +2422,15 @@ version = "1.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6ce2be8dc25455e1f91df71bfa12ad37d7af1092ae736f3a6cd0e37bc7810596" +[[package]] +name = "static_cell" +version = "2.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0530892bb4fa575ee0da4b86f86c667132a94b74bb72160f58ee5a4afec74c23" +dependencies = [ + "portable-atomic", +] + [[package]] name = "strsim" version = "0.11.1" @@ -2498,6 +2620,39 @@ dependencies = [ "tracing-log", ] +[[package]] +name = "trouble-host" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1df7817cead4b83dfbeeaa59736ecbc97c30b21dc3bfc0cc2ce8a6687f70b37a" +dependencies = [ + "bt-hci", + "embassy-futures", + "embassy-sync 0.7.2", + "embassy-time", + "embedded-io 0.7.1", + "futures", + "heapless 0.9.2", + "rand_core 0.6.4", + "static_cell", + "trouble-host-macros", + "zerocopy", +] + +[[package]] +name = "trouble-host-macros" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2d809ae05f510cf8f1d863749281eb28e049d9ae4ad52d92ea7a0ac068a0e17c" +dependencies = [ + "convert_case", + "darling 0.20.11", + "proc-macro2", + "quote", + "syn 2.0.117", + "uuid", +] + [[package]] name = "typenum" version = "1.20.0" @@ -2539,6 +2694,12 @@ version = "1.0.24" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75" +[[package]] +name = "unicode-segmentation" +version = "1.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9629274872b2bfaf8d66f5f15725007f635594914870f65218920345aa11aa8c" + [[package]] name = "unicode-width" version = "0.1.14" @@ -2570,6 +2731,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ddd74a9687298c6858e9b88ec8935ec45d22e8fd5e6394fa1bd4e99a87789c76" dependencies = [ "getrandom", + "js-sys", + "wasm-bindgen", ] [[package]] @@ -2875,6 +3038,26 @@ dependencies = [ "syn 2.0.117", ] +[[package]] +name = "zerocopy" +version = "0.8.49" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bce33a6288fa3f072a8c2c7d0f2fdbb90e28298f0135c1f99b96c3db2efcc60b" +dependencies = [ + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.8.49" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fd425244944f4ab65ccff928e7323354c5a018c75838362fdce749dfad2ee1e" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.117", +] + [[package]] name = "zmij" version = "1.0.21" diff --git a/Cargo.toml b/Cargo.toml index 9b3cbd9..ff6cc58 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -23,6 +23,8 @@ esp = [ "dep:esp-rtos", "dep:esp-bootloader-esp-idf", "dep:embassy-executor", + "dep:embassy-futures", + "dep:embassy-sync", "dep:embassy-time", "dep:embedded-io", "dep:embedded-io-async", @@ -30,6 +32,7 @@ esp = [ "dep:esp-backtrace", "dep:esp-println", "dep:esp-radio", + "dep:bt-hci", "esp32-cits-core/esp32c5", "esp32-cits-core/log-04", ] @@ -52,7 +55,7 @@ spat_debug = [] # enable ST7789 based display screen = ["dep:mipidsi", "dep:embedded-graphics", "dep:embedded-hal-bus", "dep:profont"] # enable I/O via UART -uart = ["_uart", "_gnss", "dep:prost"] +uart = ["_uart", "_gnss"] # internal feature, that any UART I/O is enabled _uart = [] @@ -66,6 +69,7 @@ esp-rtos = { version = "0.3.0", optional = true, features = [ "embassy", "esp-alloc", "esp-radio", + "embassy", "esp32c5", "log-04", ] } @@ -74,7 +78,10 @@ esp-bootloader-esp-idf = { version = "0.5.0", optional = true, features = ["esp3 log = "0.4.27" critical-section = "1.2.0" +bt-hci = { version = "0.8.0", optional = true } embassy-executor = { version = "0.10.0", optional = true, features = ["log"] } +embassy-futures = { version = "0.1.2", optional = true } +embassy-sync = { version = "0.7", optional = true } embassy-time = { version = "0.5.0", optional = true, features = ["log"] } embedded-io = { version = "0.7.1", optional = true } embedded-io-async = { version = "0.7.0", optional = true } @@ -92,7 +99,10 @@ esp-radio = { version = "0.18.0", optional = true, features = [ "unstable", "wifi", "sniffer", + "ble", ] } +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" } @@ -111,7 +121,7 @@ geo-types = { version = "0.7.19", default-features = false } mipidsi = { version = "0.10.0", optional = true } num-traits = { version = "0.2.19", default-features = false, features = ["libm"] } profont = { version = "0.7.0", optional = true } -prost = { version = "0.14", optional = true, default-features = false, features = ["derive"] } +prost = { version = "0.14", default-features = false, features = ["derive"] } ublox = { version = "0.10", optional = true, default-features = false, features = ["alloc", "ubx_proto27"] } [dev-dependencies] diff --git a/Readme.md b/Readme.md index 2ae3aab..58a5e16 100644 --- a/Readme.md +++ b/Readme.md @@ -70,7 +70,7 @@ rustup target add riscv32imac-unknown-none-elf cargo install espflash --locked ``` -(Project was setup using esp-generate with the options `--chip esp32c5 -o esp32c5-wroom-1-psram -o alloc -o log -o unstable-hal -o wifi -o esp-backtrace -o embassy`.) +(Project was setup using esp-generate with the options `--chip esp32c5 -o esp32c5-wroom-1-psram -o alloc -o log -o unstable-hal -o wifi -o esp-backtrace -o embassy -o ble-trouble`.) To use the proper rustfmt features, enable the nightly toolchain: ```shell diff --git a/docs/ble-protocol.md b/docs/ble-protocol.md index 65f68ff..639f465 100644 --- a/docs/ble-protocol.md +++ b/docs/ble-protocol.md @@ -35,15 +35,65 @@ All data types are defined in one `.proto` file: [c_its-messages.proto](./c_its- When there's no GNSS receiver connected to the ESP, position, heading and speed information can be set using a `PositionState` message (see [c_its-messages.proto](./c_its-messages.proto)). +Maximum message size (assuming realistic value ranges): + +field name | tag size | (max.) data size +----------------- | -------- | --------- +timestamp_ms | 1 | 8 (I64) +position | 1 | 1 (LEN(10)) + sub-fields below +\+ latitude_deg | 1 | 4 (I32) +\+ longitude_deg | 1 | 4 (I32) +heading | 1 | 2 (VARINT(3600)) +speed | 1 | 2 (VARINT(~163 m/s)) +**SUM** | 6 + | 21 = 27 byte + + ### DENM Event Each received DENM will be "published" as a `DenmEvent` message (see [c_its-messages.proto](./c_its-messages.proto)). Repetitions of the same message (e.g. no changes to the validity timeframe and location) may be dropped by the BLE server. +Maximum message size (assuming realistic value ranges): + +field name | tag size | (max.) data size +----------------- | -------- | --------- +timestamp | 1 | 8 (I64) +validity_end_ts | 1 | 8 (I64) +station_id | 1 | 8 (I64) +seq_num | 1 | 3 (VARINT(65535)) +event_position | 1 | 1 (LEN(10)) + sub-fields below +\+ latitude_deg | 1 | 4 (I32) +\+ longitude_deg | 1 | 4 (I32) +event_heading_deg | 1 | 1 (VARINT(360)) +direction | 1 | 1 (VARINT(3)) +cause_code | 1 | 1 (VARINT(127)) +sub_cause_code | 1 | 1 (VARINT(127)) +**SUM** | 11 + | 40 = 51 byte + +Note: Cause code and sub-cause code are technically 0..255 in the ASN.1 spec, but that's mainly to ensure backwards-compatibility of the CHOICE with the old wire format. +Realistically only cause codes up to 100 are defined and common sub-cause codes use values up to 22. + ### CAM Event Each received (low-frequency) CAM will be "published" as a `CamEvent` message (see [c_its-messages.proto](./c_its-messages.proto)). +Maximum message size (assuming realistic value ranges): + +field name | tag size | (max.) data size +--------------------- | -------- | --------- +station_id | 1 | 8 (I64) +station_type | 1 | 1 (VARINT(15)) +position | 1 | 1 (LEN(10)) + sub-fields below +\+ latitude_deg | 1 | 4 (I32) +\+ longitude_deg | 1 | 4 (I32) +vehicle_role | 1 | 1 (VARINT(15)) +vehicle_data | 1 | 1 (LEN(11)) + sub-fields below +\+ heading_deg | 1 | 2 (VARINT(360)) +\+ speed | 1 | 2 (VARINT(~163 m/s)) +\+ vehicle_length_dm | 1 | 2 (VARINT(1023)) +\+ vehicle_width_dm | 1 | 1 (VARINT(62)) +**SUM** | 11 + | 27 = 38 byte + ### Raw Received Message Received V2X messages will be "published" as a `RawMsgRx` message (see [c_its-messages.proto](./c_its-messages.proto)). diff --git a/docs/c_its-messages.proto b/docs/c_its-messages.proto index af8f802..47e523d 100644 --- a/docs/c_its-messages.proto +++ b/docs/c_its-messages.proto @@ -83,7 +83,7 @@ message PositionState { } message DenmEvent { - required fixed64 timestamp = 1; // DENM reference time as UNIX time + required fixed64 timestamp = 1; // DENM reference time as UNIX time (in seconds) required fixed64 validity_end_ts = 2; // UNIX time when event validity ends (from denm.management.validityDuration) required fixed32 station_id = 3; // unique ID of the event when combined with `seq_num` required uint32 seq_num = 4; // unique ID of the event when combined with `station_id` diff --git a/src/ble.rs b/src/ble.rs new file mode 100644 index 0000000..9bdb978 --- /dev/null +++ b/src/ble.rs @@ -0,0 +1,256 @@ +#![allow( + clippy::needless_borrows_for_generic_args, + reason = "gatt_service creates false positives" +)] + +use core::cell::RefCell; + +use critical_section::Mutex; +use esp_backtrace as _; +use log::{info, warn}; +use trouble_host::prelude::*; + +pub const CONTROLLER_SLOTS: usize = 20; +const CONNECTIONS_MAX: usize = 1; +const L2CAP_CHANNELS_MAX: usize = 1; + +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")] + Cam(crate::io::msg::CamEvent), + #[cfg(feature = "denm")] + Denm(crate::io::msg::DenmEvent), +} + +#[gatt_server] +struct Server { + c_its_events: CITSEvents, +} + +#[cfg(feature = "cam")] +const CAM_EVENT_SIZE: usize = 38; +#[cfg(feature = "denm")] +const DENM_EVENT_SIZE: usize = 51; + +#[gatt_service(uuid = "c0b70000-d4f4-4000-ada8-f99a02ee315c")] +struct CITSEvents { + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "Position", read, value = "PositionState proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4001-ada8-f99a02ee315c", write)] + position: [u8; 27], + + #[cfg(feature = "cam")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "CAM_Event", read, value = "CamEvent proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4002-ada8-f99a02ee315c", read, notify, value = [0u8; CAM_EVENT_SIZE])] + cam_event: [u8; CAM_EVENT_SIZE], + + #[cfg(feature = "denm")] + #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, name = "DENM_Event", read, value = "DenmEvent proto msg", type = &'static str)] + #[characteristic(uuid = "c0b70001-d4f4-4004-ada8-f99a02ee315c", read, notify, value = [0u8; DENM_EVENT_SIZE])] + denm_event: [u8; DENM_EVENT_SIZE], +} + +/// Creates the BLE controller +pub fn make_controller( + bt_peripheral: esp_hal::peripherals::BT<'static>, +) -> bt_hci::controller::ExternalController< + esp_radio::ble::controller::BleConnector<'static>, + CONTROLLER_SLOTS, +> { + let transport = esp_radio::ble::controller::BleConnector::new( + bt_peripheral, + esp_radio::ble::Config::default(), + ) + .unwrap(); + bt_hci::controller::ExternalController::<_, { CONTROLLER_SLOTS }>::new(transport) +} + +/// Run the BLE stack. +#[embassy_executor::task] +pub async fn run( + controller: bt_hci::controller::ExternalController< + esp_radio::ble::controller::BleConnector<'static>, + CONTROLLER_SLOTS, + >, +) { + let mut resources: HostResources = + HostResources::new(); + let stack = trouble_host::new(controller, &mut resources); + let Host { + mut peripheral, + runner, + .. + } = stack.build(); + + info!("Starting advertising and GATT service"); + let server = Server::new_with_config(GapConfig::Peripheral(PeripheralConfig { + name: "ESP32 C-ITS", + appearance: &appearance::UNKNOWN, + })) + .unwrap(); + + let _ = embassy_futures::join::join(ble_task(runner), async { + loop { + match advertise("ESP32 C-ITS", &mut peripheral, &server).await { + Ok(conn) => { + // set up tasks when the connection is established to a central, so they don't run when no one is connected. + let a = gatt_events_task(&server, &conn); + let b = custom_task(&server, &conn, &stack); + // run until any task ends (usually because the connection has been closed), + // then return to advertising state. + embassy_futures::select::select(a, b).await; + } + Err(e) => { + panic!("BLE ADV: fatal error {e:?}"); + } + } + } + }) + .await; +} + +/// Create an advertiser to use to connect to a BLE Central, and wait for it to connect. +async fn advertise<'values, 'server, C: Controller>( + name: &'values str, + peripheral: &mut Peripheral<'values, C, DefaultPacketPool>, + server: &'server Server<'values>, +) -> Result, BleHostError> { + let mut advertiser_data = [0; 35]; + let len = AdStructure::encode_slice( + &[ + AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED), + AdStructure::CompleteLocalName(name.as_bytes()), + ], + &mut advertiser_data[..], + )?; + + let advertiser = peripheral + .advertise( + &trouble_host::advertise::AdvertisementParameters { + timeout: Some(embassy_time::Duration::from_secs(2)), + ..Default::default() + }, + Advertisement::ConnectableScannableUndirected { + adv_data: &advertiser_data[..len], + scan_data: &[], + }, + ) + .await?; + // info!("[adv] advertising {len} bytes"); + let conn = advertiser.accept().await?.with_attribute_server(server)?; + // info!("[adv] connection established"); + Ok(conn) +} + +async fn ble_task(mut runner: Runner<'_, C, P>) { + loop { + if let Err(e) = runner.run().await { + panic!("BLE Task: fatal error {e:?}"); + } + } +} + +/// Stream Events until the connection closes. +/// +/// This function will handle the GATT events and process them. +/// This is how we interact with read and write requests. +async fn gatt_events_task( + server: &Server<'_>, + conn: &GattConnection<'_, '_, P>, +) -> Result<(), Error> { + let reason = loop { + match conn.next().await { + GattConnectionEvent::Disconnected { reason } => break reason, + GattConnectionEvent::Gatt { event } => { + match &event { + GattEvent::Write(event) + if event.handle() == server.c_its_events.position.handle => + { + info!("[gatt] Write Event to Position: {:?}", event.data()); // TODO dev only + // TODO: parse proto and send to main thread + } + + _ => {} + } + + // This step is also performed at drop(), but writing it explicitly is necessary + // in order to ensure reply is sent. + match event.accept() { + Ok(reply) => reply.send().await, + Err(e) => warn!("BLE: error sending response: {e:?}"), + } + } + _ => {} // ignore other Gatt Connection Events + } + }; + + info!("BLE: disconnected b/c {reason:?}"); + Ok(()) +} + +/// Example task to use the BLE notifier interface. +/// This task will notify the connected central of a counter value every 2 seconds. +/// It will also read the RSSI value every 2 seconds. +/// and will stop when the connection is closed by the central or an error occurs. +async fn custom_task( + server: &Server<'_>, + conn: &GattConnection<'_, '_, P>, + stack: &Stack<'_, C, P>, +) { + loop { + embassy_time::Timer::after(embassy_time::Duration::from_millis(10)).await; + + let mut new_data = None; + critical_section::with(|cs| { + let tx_data_ref = TX_DATA.borrow(cs); + + if let Some(tx_data) = tx_data_ref.replace(None) { + new_data = Some(tx_data); + } + }); + + match new_data { + #[cfg(feature = "cam")] + Some(BleSendable::Cam(cam_event)) => { + let mut buf = cam_event.serialize_ble_proto(); + + if buf.len() <= CAM_EVENT_SIZE { + // pad with zeros + let pad = alloc::vec![0u8; CAM_EVENT_SIZE - buf.len()]; + buf.extend_from_slice(&pad); + + // unwrap should be fine since we ensured the vector size before + let value = buf.try_into().unwrap(); + + if let Err(err) = server.c_its_events.cam_event.notify(conn, &value).await { + warn!("BLE: failed notifying connection of new CAM event: {err:?}"); + } + } else { + warn!("BLE: CAM event too big with {} bytes", buf.len()); + } + } + #[cfg(feature = "denm")] + Some(BleSendable::Denm(denm_event)) => { + let mut buf = denm_event.serialize_ble_proto(); + + if buf.len() <= DENM_EVENT_SIZE { + // pad with zeros + let pad = alloc::vec![0u8; DENM_EVENT_SIZE - buf.len()]; + buf.extend_from_slice(&pad); + + // unwrap should be fine since we ensured the vector size before + let value = buf.try_into().unwrap(); + + if let Err(err) = server.c_its_events.denm_event.notify(conn, &value).await { + warn!("BLE: failed notifying connection of new DENM event: {err:?}"); + } + } else { + warn!("BLE: DENM event too big with {} bytes", buf.len()); + } + } + None => {} + } + } +} diff --git a/src/io.rs b/src/io.rs index 44997ee..ab55a2c 100644 --- a/src/io.rs +++ b/src/io.rs @@ -5,12 +5,12 @@ pub mod msg { include!(concat!(env!("OUT_DIR"), "/c_its_io.rs")); } -use alloc::string::ToString; use alloc::vec::Vec; +#[cfg(feature = "uart")] use c_its_parser::gn as geonetworking; -use log::error; +#[cfg(feature = "uart")] impl From for msg::SerialOutputMsg { fn from(value: msg::RawMsgRx) -> Self { use msg::serial_output_msg::Payload; @@ -21,6 +21,7 @@ impl From for msg::SerialOutputMsg { } } +#[cfg(feature = "uart")] impl From for msg::SerialOutputMsg { fn from(value: msg::DenmEvent) -> Self { use msg::serial_output_msg::Payload; @@ -32,22 +33,41 @@ impl From for msg::SerialOutputMsg { } impl msg::RawMsgRx { + #[cfg(feature = "uart")] pub fn serialize_uart_proto(&self) -> Vec { serialize_serial_msg(&Into::::into(self.clone())) } } impl msg::DenmEvent { + #[cfg(feature = "uart")] #[allow(unused)] pub fn serialize_uart_proto(&self) -> Vec { serialize_serial_msg(&Into::::into(*self)) } + + #[cfg(feature = "denm")] + pub fn serialize_ble_proto(&self) -> Vec { + use prost::Message as _; + + self.encode_to_vec() + } +} +impl msg::CamEvent { + #[cfg(feature = "cam")] + pub fn serialize_ble_proto(&self) -> Vec { + use prost::Message as _; + + self.encode_to_vec() + } } +#[cfg(feature = "uart")] #[derive(Debug, Default)] pub struct Parser { buffer: alloc::vec::Vec, } +#[cfg(feature = "uart")] impl Parser { const SOF_LEN: u16 = 4; const SOF1: u8 = 0x56; @@ -91,7 +111,7 @@ impl Parser { Ok(data) => { output.push(data); } - Err(err) => error!("Failed to parse input: {err:?}"), + Err(err) => log::error!("Failed to parse input: {err:?}"), } // consume buffer @@ -122,6 +142,7 @@ impl Parser { } } +#[cfg(feature = "uart")] fn serialize_serial_msg(msg: &T) -> Vec where T: prost::Message, @@ -141,6 +162,7 @@ where out } +#[cfg(feature = "uart")] impl From for crate::applogic::GnssFix { fn from(value: msg::PositionState) -> Self { let time = @@ -169,6 +191,7 @@ impl From for crate::applogic::GnssFix { } } +#[cfg(feature = "uart")] impl From for esp32_cits_core::PosVel { fn from(value: msg::PositionState) -> Self { let position = geo_types::Point::new( @@ -194,6 +217,7 @@ impl From for esp32_cits_core::PosVel { } } +#[cfg(feature = "uart")] impl From for esp32_cits_core::PosVel { fn from(value: msg::ItsPosition) -> Self { let latitude_deg = f64::from(value.latitude) / 10_000_000.; @@ -208,6 +232,7 @@ impl From for esp32_cits_core::PosVel { } } +#[cfg(feature = "uart")] impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx { fn make_eh( &self, @@ -223,6 +248,8 @@ impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx { ), alloc::string::String, > { + use alloc::string::ToString as _; + let station_type = self .station_type .and_then(|v| { @@ -304,6 +331,7 @@ impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx { } } +#[cfg(feature = "uart")] impl msg::RawMsgTx { fn make_geobcast( address: [u8; 6], @@ -384,12 +412,189 @@ impl msg::RawMsgTx { } } +#[cfg(feature = "cam")] +impl From> + for msg::CamEvent +{ + fn from( + value: alloc::boxed::Box, + ) -> Self { + use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions; + + let station_id = value.header.station_id.0; + let station_type = u32::from(value.cam.cam_parameters.basic_container.station_type.0); + let position = value + .cam + .cam_parameters + .basic_container + .reference_position + .into(); + + let vehicle_role = value + .cam + .cam_parameters + .low_frequency_container + .and_then(|v| match v { + cam_pdu_descriptions::LowFrequencyContainer::basicVehicleContainerLowFrequency( + basic_vehicle_container_low_frequency, + ) => Some(basic_vehicle_container_low_frequency.vehicle_role as u32), + _ => None, + }); + + let vehicle_data = match value.cam.cam_parameters.high_frequency_container { + cam_pdu_descriptions::HighFrequencyContainer::basicVehicleContainerHighFrequency( + basic_vehicle_container_high_frequency, + ) => Some(basic_vehicle_container_high_frequency.into()), + _ => None, + }; + + Self { + station_id, + station_type, + position, + vehicle_role, + vehicle_data, + } + } +} + +#[cfg(feature = "cam")] +impl From for msg::VehicleContainer { + fn from(value: c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency) -> Self { + let heading_deg = if value.heading.heading_value.is_unavailable() { + None + } else { + Some(u32::from(value.heading.heading_value.0)) + }; + let speed = if value.speed.speed_value.is_unavailable() { + None + } else { + Some(u32::from(value.speed.speed_value.0)) + }; + let vehicle_length_dm = if value.vehicle_length.vehicle_length_value.is_unavailable() { + None + } else { + Some(u32::from(value.vehicle_length.vehicle_length_value.0)) + }; + let vehicle_width_dm = if value.vehicle_width.is_unavailable() { + None + } else { + Some(u32::from(value.vehicle_width.0)) + }; + + + Self { heading_deg, speed, vehicle_length_dm, vehicle_width_dm } + } +} + +#[cfg(feature = "denm")] +impl From> + for msg::DenmEvent +{ + fn from( + value: alloc::boxed::Box, + ) -> Self { + let denm_mgmt = &value.denm.management; + + let station_id = value.header.station_id.0; // TODO: or from action ID? + + let ref_time = chrono::DateTime::::from(denm_mgmt.reference_time.clone()); + let end_time = ref_time + chrono::Duration::seconds(denm_mgmt.validity_duration.0.into()); + + #[allow(clippy::cast_sign_loss)] + let timestamp = ref_time.timestamp_millis() as u64; + #[allow(clippy::cast_sign_loss)] + let validity_end_ts = end_time.timestamp_millis() as u64; + let seq_num = denm_mgmt.action_id.sequence_number.0.into(); + + let event_position = Some(denm_mgmt.event_position.clone().into()); + let event_heading_deg = value + .denm + .location + .and_then(|v| v.event_position_heading) + .map(|v| { + #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)] + let conv = v.value.as_deg() as u32; + conv + }); + let direction = denm_mgmt + .traffic_direction + .map(|v| msg::TrafficDir::from(v) as i32); + let cc_tuple = value + .denm + .situation + .map(|v| v.event_type.cc_and_scc.to_u8_tuple()); + let cause_code = cc_tuple.map(|v| v.0.into()); + let sub_cause_code = cc_tuple.map(|v| v.1.into()); + + Self { + timestamp, + validity_end_ts, + station_id, + seq_num, + event_position, + event_heading_deg, + direction, + cause_code, + sub_cause_code, + } + } +} + +impl From + for msg::Position +{ + fn from(value: c_its_parser::standards::cdd_1_3_1_1::its_container::ReferencePosition) -> Self { + #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)] + let latitude_deg = value.latitude.as_deg() as f32; + #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)] + let longitude_deg = value.longitude.as_deg() as f32; + + Self { + latitude_deg, + longitude_deg, + } + } +} + +impl From for msg::Position { + fn from(value: c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::ReferencePosition) -> Self { + #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)] + let latitude_deg = value.latitude.as_deg() as f32; + #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)] + let longitude_deg = value.longitude.as_deg() as f32; + + Self { + latitude_deg, + longitude_deg, + } + } +} + +impl From for msg::TrafficDir { + fn from(value: c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection) -> Self { + use c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection; + + match value { + TrafficDirection::allTrafficDirections => Self::AllDirections, + TrafficDirection::sameAsReferenceDirection_upstreamOfReferencePosition => { + Self::Upstream + } + TrafficDirection::sameAsReferenceDirection_downstreamOfReferencePosition => { + Self::Downstream + } + TrafficDirection::oppositeToReferenceDirection => Self::Opposite, + } + } +} + #[cfg(test)] mod tests { use super::*; use crate::init_test_env_logger; + #[cfg(feature = "uart")] impl From for msg::SerialInputMsg { fn from(value: msg::RawMsgTx) -> Self { use msg::serial_input_msg::Payload; @@ -400,6 +605,7 @@ mod tests { } } + #[cfg(feature = "uart")] impl From for msg::SerialInputMsg { fn from(value: msg::PositionState) -> Self { use msg::serial_input_msg::Payload; @@ -414,6 +620,7 @@ mod tests { // SerialInputMsg(RawMsgTx): 5632582b0040123e0a340202c173256817284029d5b3400f1feaf01ffffffc23b7743e00000fc0007e8138250737feebfff600000dffff7ffff1ce40400010011807220028020d0a // SerialInputMsg(PositionState): 5632582b00170a1509a0bbe2a29f010000120a0d00002842150000b8410d0a + #[cfg(feature = "uart")] #[test] fn uart_parser() { // generate some test UART input messages diff --git a/src/main.rs b/src/main.rs index 67e181c..a723f90 100644 --- a/src/main.rs +++ b/src/main.rs @@ -45,9 +45,10 @@ use log::{error, info, warn}; extern crate alloc; mod applogic; +#[cfg(any(feature = "cam", feature = "denm"))] +mod ble; mod cache; mod geo_alg; -#[cfg(feature = "uart")] mod io; #[cfg(feature = "screen")] mod screen; @@ -89,7 +90,7 @@ static SERIAL: Mutex ! { // generator version: 1.3.0 - // generator parameters: --chip esp32c5 -o esp32c5-wroom-1-psram -o alloc -o log -o unstable-hal -o wifi -o esp-backtrace -o embassy + // generator parameters: --chip esp32c5 -o esp32c5-wroom-1-psram -o alloc -o log -o unstable-hal -o wifi -o esp-backtrace -o embassy -o ble-trouble esp_println::logger::init_logger_from_env(); @@ -107,6 +108,8 @@ async fn main(spawner: Spawner) -> ! { // - GPIO28 esp_alloc::heap_allocator!(#[esp_hal::ram(reclaimed)] size: 65536); + // COEX needs more RAM - so we've added some more + esp_alloc::heap_allocator!(size: 64 * 1024); esp_alloc::psram_allocator!(peripherals.PSRAM, esp_hal::psram); let timg0 = TimerGroup::new(peripherals.TIMG0); @@ -188,6 +191,13 @@ async fn main(spawner: Spawner) -> ! { let mac = esp_hal::efuse::base_mac_address(); info!("WiFi MAC: {mac}"); + // Setup BLE + #[cfg(any(feature = "cam", feature = "denm"))] + { + let controller = ble::make_controller(peripherals.BT); + spawner.spawn(ble::run(controller).expect("Failed to spawn BLE task")); + } + // Setup GNSS: // Seeed XIAO L67K: // RX: D7/ GPIO12 @@ -474,6 +484,13 @@ async fn main(spawner: Spawner) -> ! { } => { #[cfg(feature = "cam")] applogic::cam::handle_cam(&etsi); + + // Send data to BLE thread + #[cfg(feature = "cam")] + critical_section::with(|cs| { + let ble_update_ref = ble::TX_DATA.borrow(cs); + ble_update_ref.replace(Some(ble::BleSendable::Cam(etsi.into()))); + }); } #[cfg(feature = "denm")] @@ -481,7 +498,15 @@ async fn main(spawner: Spawner) -> ! { geonetworking: _, transport: _, etsi, - } => applogic::denm::handle_denm(&etsi), + } => { + applogic::denm::handle_denm(&etsi); + + // Send data to BLE thread + critical_section::with(|cs| { + let ble_update_ref = ble::TX_DATA.borrow(cs); + ble_update_ref.replace(Some(ble::BleSendable::Denm(etsi.into()))); + }); + } #[cfg(feature = "denm")] ItsMessage::DenmV1 { geonetworking: _,