Compare commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 8f7525a9f3 |
10 changed files with 106 additions and 562 deletions
30
Cargo.lock
generated
30
Cargo.lock
generated
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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 }
|
||||
|
|
|
|||
10
Readme.md
10
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
// ------------------------------
|
||||
|
|
|
|||
|
|
@ -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
|
||||
)
|
||||
|
|
|
|||
|
|
@ -118,7 +118,6 @@ pub struct TimingData {
|
|||
pub max_end_sec: Option<i16>,
|
||||
}
|
||||
|
||||
#[cfg(feature = "spat")]
|
||||
impl TimingData {
|
||||
pub fn new(value: &map::SignalGroup, now: chrono::DateTime<chrono::Utc>) -> Option<Self> {
|
||||
let min = value
|
||||
|
|
|
|||
263
src/ble.rs
263
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, ¬ify_buf)
|
||||
.await
|
||||
.map_err(|err| alloc::format!("failed notifying connection of new raw {msg_id:?}: {err:?}"))
|
||||
}
|
||||
|
|
|
|||
146
src/io.rs
146
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 {
|
||||
|
||||
|
|
|
|||
176
src/main.rs
176
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>) {
|
||||
|
|
|
|||
Loading…
Reference in a new issue