Compare commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
afe19e7f3e |
16 changed files with 218 additions and 1505 deletions
183
Cargo.lock
generated
183
Cargo.lock
generated
|
|
@ -166,29 +166,6 @@ 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"
|
||||
|
|
@ -267,15 +244,6 @@ 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"
|
||||
|
|
@ -988,7 +956,6 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
|
|||
checksum = "23fbff98b06a96b6ce3791ecec5c668524052a068e23aacd23afe17ddba844ce"
|
||||
dependencies = [
|
||||
"allocator-api2",
|
||||
"bt-hci",
|
||||
"cfg-if",
|
||||
"docsplay",
|
||||
"document-features",
|
||||
|
|
@ -1185,14 +1152,11 @@ 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",
|
||||
|
|
@ -1215,8 +1179,6 @@ dependencies = [
|
|||
"profont",
|
||||
"prost",
|
||||
"prost-build",
|
||||
"static_cell",
|
||||
"trouble-host",
|
||||
"ublox",
|
||||
]
|
||||
|
||||
|
|
@ -1358,52 +1320,12 @@ 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"
|
||||
|
|
@ -1423,10 +1345,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
|
|||
checksum = "389ca41296e6190b48053de0321d02a77f32f8a5d2461dd38762c0593805c6d6"
|
||||
dependencies = [
|
||||
"futures-core",
|
||||
"futures-sink",
|
||||
"futures-task",
|
||||
"pin-project-lite",
|
||||
"slab",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -1641,18 +1561,6 @@ 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"
|
||||
|
|
@ -1695,15 +1603,6 @@ 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"
|
||||
|
|
@ -2285,12 +2184,6 @@ 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"
|
||||
|
|
@ -2368,12 +2261,6 @@ 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"
|
||||
|
|
@ -2422,15 +2309,6 @@ 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"
|
||||
|
|
@ -2620,39 +2498,6 @@ 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"
|
||||
|
|
@ -2694,12 +2539,6 @@ 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"
|
||||
|
|
@ -2731,8 +2570,6 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
|
|||
checksum = "ddd74a9687298c6858e9b88ec8935ec45d22e8fd5e6394fa1bd4e99a87789c76"
|
||||
dependencies = [
|
||||
"getrandom",
|
||||
"js-sys",
|
||||
"wasm-bindgen",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -3038,26 +2875,6 @@ 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"
|
||||
|
|
|
|||
23
Cargo.toml
23
Cargo.toml
|
|
@ -14,6 +14,12 @@ name = "test"
|
|||
path = "./src/test.rs"
|
||||
required-features = ["std", "cam"]
|
||||
|
||||
[[bin]]
|
||||
## run with cargo run --target host-tuple --bin test-proto --no-default-features -F std,uart,cam
|
||||
name = "test-proto"
|
||||
path = "./src/test-proto.rs"
|
||||
required-features = ["std", "uart", "cam"]
|
||||
|
||||
[features]
|
||||
default = ["esp", "spat", "gnss_ubx", "screen"]
|
||||
|
||||
|
|
@ -23,8 +29,6 @@ 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",
|
||||
|
|
@ -32,7 +36,6 @@ esp = [
|
|||
"dep:esp-backtrace",
|
||||
"dep:esp-println",
|
||||
"dep:esp-radio",
|
||||
"dep:bt-hci",
|
||||
"esp32-cits-core/esp32c5",
|
||||
"esp32-cits-core/log-04",
|
||||
]
|
||||
|
|
@ -55,9 +58,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"]
|
||||
# enable I/O via BLE
|
||||
ble = ["_gnss"]
|
||||
uart = ["_uart", "_gnss", "dep:prost"]
|
||||
|
||||
# internal feature, that any UART I/O is enabled
|
||||
_uart = []
|
||||
|
|
@ -71,7 +72,6 @@ esp-rtos = { version = "0.3.0", optional = true, features = [
|
|||
"embassy",
|
||||
"esp-alloc",
|
||||
"esp-radio",
|
||||
"embassy",
|
||||
"esp32c5",
|
||||
"log-04",
|
||||
] }
|
||||
|
|
@ -80,16 +80,14 @@ 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 }
|
||||
esp-alloc = { version = "0.10.0", optional = true }
|
||||
esp-backtrace = { version = "0.19.0", optional = true, features = [
|
||||
"esp32c5",
|
||||
"panic-handler",
|
||||
"println",
|
||||
] }
|
||||
esp-println = { version = "0.17.0", optional = true, default-features = false, features = ["colors", "critical-section", "jtag-serial", "esp32c5", "log-04"] }
|
||||
|
|
@ -100,10 +98,7 @@ 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" }
|
||||
|
|
@ -122,7 +117,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", default-features = false, features = ["derive"] }
|
||||
prost = { version = "0.14", optional = true, default-features = false, features = ["derive"] }
|
||||
ublox = { version = "0.10", optional = true, default-features = false, features = ["alloc", "ubx_proto27"] }
|
||||
|
||||
[dev-dependencies]
|
||||
|
|
|
|||
14
Readme.md
14
Readme.md
|
|
@ -8,7 +8,7 @@ The code is currently tailored to:
|
|||
- U-Blox M10Q GNSS (connected on GPIO12 (RX), GPIO11 (TX), and GPIO25 (reset))
|
||||
or Seeed XIAO L67K (connected on GPIO12 (RX), GPIO11 (TX), GPIO1 (wakeup) and GPIO25 (reset))
|
||||
only available with `gnss_ubx` or `gnss_l67k` feature (which is included in `spat` feature)
|
||||
- Some ST7789 240*280px screen (connected on GPIO8 (SCL/ SCK), GPIO10 (SDA/ MOSI), GPIO7 (reset), GPIO23 (DC), GPIO24 (CS), GPIO0 (BL))
|
||||
- Some ST7789 172*320px screen (connected on GPIO8 (SCL/ SCK), GPIO10 (SDA/ MOSI), GPIO7 (reset), GPIO23 (DC), GPIO24 (CS), TODO GPIO0 (BL))
|
||||
only available with `screen` feature (which is included in `spat` feature)
|
||||
|
||||
## Features
|
||||
|
|
@ -55,13 +55,6 @@ This is **not** implemented in this application!
|
|||
### UART
|
||||
Enable UART I/O protocol according to [docs/uart-protocol.md](./docs/uart-protocol.md).
|
||||
|
||||
### 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.
|
||||
|
||||
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.
|
||||
|
||||
|
||||
## Usage
|
||||
|
||||
|
|
@ -77,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 -o ble-trouble`.)
|
||||
(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`.)
|
||||
|
||||
To use the proper rustfmt features, enable the nightly toolchain:
|
||||
```shell
|
||||
|
|
@ -100,8 +93,7 @@ Officially supported feature combinations are:
|
|||
- `esp,spat,gnss_ubx,screen` (default features, "standalone")
|
||||
- `esp,spat,gnss_ubx,screen,cam_tx` ("standalone with CAM")
|
||||
- `esp,spat,gnss_ubx,screen,cam_tx,cam,denm`
|
||||
- `esp,uart,cam,denm` ("smartphone companion with UART")
|
||||
- `esp,ble,cam,denm,cam_tx` ("smartphone companion with BLE")
|
||||
- `esp,uart,cam,denm` ("smartphone companion")
|
||||
|
||||
Connect your ESP32-C5 and run:
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -39,65 +35,15 @@ 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)).
|
||||
|
|
@ -105,20 +51,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
|
||||
// ------------------------------
|
||||
|
|
@ -87,7 +83,7 @@ message PositionState {
|
|||
}
|
||||
|
||||
message DenmEvent {
|
||||
required fixed64 timestamp = 1; // DENM reference time as UNIX time (in seconds)
|
||||
required fixed64 timestamp = 1; // DENM reference time as UNIX time
|
||||
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`
|
||||
|
|
|
|||
|
|
@ -9,14 +9,10 @@ extended=${2:-false}
|
|||
|
||||
features_base=(
|
||||
uart,cam,denm
|
||||
ble,cam,denm,cam_tx
|
||||
uart,ble,cam,denm
|
||||
spat,gnss_ubx,screen
|
||||
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
|
||||
)
|
||||
|
|
|
|||
|
|
@ -1,6 +0,0 @@
|
|||
#!/bin/sh
|
||||
|
||||
## Builds common config for smartphone connected device
|
||||
## -> receive CAM and DENMs via BLE and send CAMs based on position data received from BLE
|
||||
|
||||
cargo run --release --no-default-features -F esp,cam,denm,ble,cam_tx
|
||||
|
|
@ -1,6 +0,0 @@
|
|||
#!/bin/sh
|
||||
|
||||
## Builds common config for smartphone connected device
|
||||
## -> only receive CAM and DENMs via UART and BLE
|
||||
|
||||
cargo run --release --no-default-features -F esp,cam,denm,uart,ble
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
#!/bin/sh
|
||||
|
||||
## Builds common config for smartphone connected device
|
||||
## -> only receive CAM and DENMs via UART
|
||||
## -> only receive CAM and DENMs via UART (TODO: and BLE)
|
||||
|
||||
cargo run --release --no-default-features -F esp,cam,denm,uart
|
||||
|
|
@ -106,52 +106,7 @@ pub struct GlosaSignalInfo {
|
|||
pub struct SignalGroupData {
|
||||
pub phase: SignalPhase,
|
||||
pub maneuver: map::Maneuvers,
|
||||
pub timing: Option<TimingData>,
|
||||
}
|
||||
|
||||
#[cfg(feature = "spat")]
|
||||
#[derive(Debug, Default, Clone, PartialEq, Eq)]
|
||||
#[allow(clippy::struct_field_names)]
|
||||
pub struct TimingData {
|
||||
pub min_end_sec: i16,
|
||||
pub likely_end_sec: Option<i16>,
|
||||
pub max_end_sec: Option<i16>,
|
||||
}
|
||||
|
||||
#[cfg(feature = "spat")]
|
||||
impl TimingData {
|
||||
pub fn new(value: &map::SignalGroup, now: chrono::DateTime<chrono::Utc>) -> Option<Self> {
|
||||
let min = value
|
||||
.min_end_time()
|
||||
.map(|val| Self::time_to_duration_sec(now, val));
|
||||
let likely_end_sec = value
|
||||
.likely_time()
|
||||
.map(|val| Self::time_to_duration_sec(now, val));
|
||||
let max_end_sec = value
|
||||
.max_end_time()
|
||||
.map(|val| Self::time_to_duration_sec(now, val));
|
||||
|
||||
min.map(|min_end_sec| Self {
|
||||
min_end_sec,
|
||||
likely_end_sec,
|
||||
max_end_sec,
|
||||
})
|
||||
}
|
||||
|
||||
fn time_to_duration_sec(
|
||||
now: chrono::DateTime<chrono::Utc>,
|
||||
time_point: chrono::DateTime<chrono::Utc>,
|
||||
) -> i16 {
|
||||
use core::ops::Sub as _;
|
||||
|
||||
use num_traits::Float as _;
|
||||
|
||||
let dur = time_point.sub(now).as_seconds_f32();
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
let duration_sec = dur.round() as i16;
|
||||
|
||||
duration_sec
|
||||
}
|
||||
pub end_sec: Option<i16>,
|
||||
}
|
||||
|
||||
#[cfg(feature = "spat")]
|
||||
|
|
@ -308,7 +263,6 @@ impl State {
|
|||
}
|
||||
|
||||
#[cfg(feature = "spat")]
|
||||
#[allow(clippy::too_many_lines)]
|
||||
pub fn run_glosa(&mut self) -> GlosaOutput {
|
||||
let Some(own_heading) = self.pos.heading_deg else {
|
||||
use alloc::string::ToString;
|
||||
|
|
@ -402,17 +356,33 @@ impl State {
|
|||
let sig_grps = approach_sig_grps
|
||||
.iter()
|
||||
.filter_map(|sig| {
|
||||
if let Some(maneuver) = sig.maneuvers() {
|
||||
if let Some(manv) = sig.maneuvers() {
|
||||
use core::ops::Sub;
|
||||
|
||||
use num_traits::Float;
|
||||
|
||||
let phase = sig
|
||||
.phase()
|
||||
.map(core::convert::Into::into)
|
||||
.unwrap_or_default();
|
||||
let timing = TimingData::new(sig, self.time.and_utc());
|
||||
|
||||
let end_sec = sig
|
||||
.likely_time()
|
||||
.or_else(|| sig.min_end_time())
|
||||
.map(|time| {
|
||||
let dur =
|
||||
time.sub(self.time.and_utc()).as_seconds_f32();
|
||||
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
let end_sec = dur.round() as i16;
|
||||
|
||||
end_sec
|
||||
});
|
||||
|
||||
Some(SignalGroupData {
|
||||
phase,
|
||||
maneuver,
|
||||
timing,
|
||||
maneuver: manv,
|
||||
end_sec,
|
||||
})
|
||||
} else {
|
||||
None
|
||||
|
|
|
|||
|
|
@ -173,12 +173,6 @@ impl Intersection {
|
|||
{
|
||||
sig_grp.likely_time = Some(likely.to_datetime_from_moy(moy, year));
|
||||
}
|
||||
if let Some(max) = &timing.max_end_time
|
||||
&& !max.is_out_of_range()
|
||||
&& !max.is_unknown()
|
||||
{
|
||||
sig_grp.max_end_time = Some(max.to_datetime_from_moy(moy, year));
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
|
|
@ -229,7 +223,6 @@ impl Intersection {
|
|||
target_lanes,
|
||||
phase: None,
|
||||
min_end_time: None,
|
||||
max_end_time: None,
|
||||
likely_time: None,
|
||||
}
|
||||
})
|
||||
|
|
@ -417,7 +410,7 @@ pub struct SignalGroup {
|
|||
/// current signal phase
|
||||
phase: Option<etsi_its_dsrc::MovementPhaseState>,
|
||||
min_end_time: Option<chrono::DateTime<chrono::Utc>>,
|
||||
max_end_time: Option<chrono::DateTime<chrono::Utc>>,
|
||||
// max_end_time: Option<chrono::DateTime<chrono::Utc>>,
|
||||
likely_time: Option<chrono::DateTime<chrono::Utc>>,
|
||||
}
|
||||
|
||||
|
|
@ -450,9 +443,6 @@ impl SignalGroup {
|
|||
pub fn likely_time(&self) -> Option<chrono::prelude::DateTime<chrono::prelude::Utc>> {
|
||||
self.likely_time
|
||||
}
|
||||
pub fn max_end_time(&self) -> Option<chrono::prelude::DateTime<chrono::prelude::Utc>> {
|
||||
self.max_end_time
|
||||
}
|
||||
|
||||
pub fn phase_to_str(&self) -> Option<String> {
|
||||
self.phase.map(|v| {
|
||||
|
|
|
|||
416
src/ble.rs
416
src/ble.rs
|
|
@ -1,416 +0,0 @@
|
|||
#![allow(
|
||||
clippy::needless_borrows_for_generic_args,
|
||||
reason = "gatt_service creates false positives"
|
||||
)]
|
||||
|
||||
use core::cell::RefCell;
|
||||
|
||||
use critical_section::Mutex;
|
||||
use crossbeam_queue::ArrayQueue;
|
||||
use esp_backtrace as _;
|
||||
use log::{info, warn};
|
||||
use trouble_host::prelude::*;
|
||||
|
||||
use crate::io;
|
||||
|
||||
pub const CONTROLLER_SLOTS: usize = 20;
|
||||
const CONNECTIONS_MAX: usize = 1;
|
||||
const L2CAP_CHANNELS_MAX: usize = 1;
|
||||
|
||||
pub static 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 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]
|
||||
struct Server {
|
||||
c_its_events: CITSEvents,
|
||||
}
|
||||
|
||||
#[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],
|
||||
|
||||
#[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],
|
||||
}
|
||||
|
||||
/// 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,
|
||||
> {
|
||||
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(),
|
||||
)
|
||||
.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,
|
||||
>,
|
||||
name: alloc::string::String,
|
||||
) {
|
||||
let mut resources: HostResources<DefaultPacketPool, CONNECTIONS_MAX, L2CAP_CHANNELS_MAX> =
|
||||
HostResources::new();
|
||||
let stack = trouble_host::new(controller, &mut resources);
|
||||
let Host {
|
||||
mut peripheral,
|
||||
runner,
|
||||
..
|
||||
} = stack.build();
|
||||
|
||||
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(",")
|
||||
);
|
||||
let server = Server::new_with_config(GapConfig::Peripheral(PeripheralConfig {
|
||||
name: &name,
|
||||
appearance: &appearance::UNKNOWN,
|
||||
}))
|
||||
.unwrap();
|
||||
|
||||
let _ = embassy_futures::join::join(ble_task(runner), async {
|
||||
loop {
|
||||
match advertise(&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.
|
||||
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:?}");
|
||||
}
|
||||
}
|
||||
}
|
||||
})
|
||||
.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>(
|
||||
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]),
|
||||
],
|
||||
&mut advertiser_data[..],
|
||||
)?;
|
||||
|
||||
let advertiser = peripheral
|
||||
.advertise(
|
||||
&trouble_host::advertise::AdvertisementParameters {
|
||||
timeout: Some(embassy_time::Duration::from_secs(2)),
|
||||
..Default::default()
|
||||
},
|
||||
Advertisement::ConnectableScannableUndirected {
|
||||
adv_data: &advertiser_data[..len],
|
||||
scan_data: &[],
|
||||
},
|
||||
)
|
||||
.await?;
|
||||
// info!("[adv] advertising {len} bytes");
|
||||
let conn = advertiser.accept().await?.with_attribute_server(server)?;
|
||||
// info!("[adv] connection established");
|
||||
Ok(conn)
|
||||
}
|
||||
|
||||
async fn ble_task<C: Controller, P: PacketPool>(mut runner: Runner<'_, C, P>) {
|
||||
loop {
|
||||
if let Err(e) = runner.run().await {
|
||||
panic!("BLE Task: fatal error {e:?}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Stream Events until the connection closes.
|
||||
///
|
||||
/// This function will handle the GATT events and process them.
|
||||
/// This is how we interact with read and write requests.
|
||||
async fn gatt_events_task<P: PacketPool>(
|
||||
server: &Server<'_>,
|
||||
conn: &GattConnection<'_, '_, P>,
|
||||
) -> Result<(), Error> {
|
||||
let reason = loop {
|
||||
match conn.next().await {
|
||||
GattConnectionEvent::Disconnected { reason } => break reason,
|
||||
GattConnectionEvent::Gatt { event } => {
|
||||
match &event {
|
||||
GattEvent::Write(event)
|
||||
if event.handle() == server.c_its_events.position.handle =>
|
||||
{
|
||||
use prost::Message as _;
|
||||
|
||||
// parse and send to main thread
|
||||
match io::msg::PositionState::decode(event.data()) {
|
||||
Ok(data) => {
|
||||
critical_section::with(|cs| {
|
||||
let rx_data_ref = RX_DATA.borrow(cs);
|
||||
let _ = rx_data_ref.replace(Some(data));
|
||||
});
|
||||
}
|
||||
Err(err) => log::error!("BLE: Failed to parse input: {err:?}"),
|
||||
}
|
||||
}
|
||||
|
||||
_ => {}
|
||||
}
|
||||
|
||||
// This step is also performed at drop(), but writing it explicitly is necessary
|
||||
// in order to ensure reply is sent.
|
||||
match event.accept() {
|
||||
Ok(reply) => reply.send().await,
|
||||
Err(e) => warn!("BLE: error sending response: {e:?}"),
|
||||
}
|
||||
}
|
||||
_ => {} // ignore other Gatt Connection Events
|
||||
}
|
||||
};
|
||||
|
||||
info!("BLE: disconnected b/c {reason:?}");
|
||||
Ok(())
|
||||
}
|
||||
|
||||
#[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);
|
||||
});
|
||||
|
||||
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();
|
||||
|
||||
if let Some(data) = queue.pop() {
|
||||
new_data = Some(data);
|
||||
}
|
||||
});
|
||||
|
||||
match new_data {
|
||||
#[cfg(feature = "cam")]
|
||||
Some(BleSendable::Cam(cam_event)) => match cam_event.serialize_ble_proto() {
|
||||
Ok(value) => {
|
||||
if let Err(err) = server.c_its_events.cam_event.notify(conn, &value).await {
|
||||
warn!("BLE: failed notifying connection of new CAM event: {err:?}");
|
||||
}
|
||||
}
|
||||
Err(err) => warn!("BLE: CAM event {err}"),
|
||||
},
|
||||
|
||||
#[cfg(feature = "denm")]
|
||||
Some(BleSendable::Denm(denm_event)) => match denm_event.serialize_ble_proto() {
|
||||
Ok(value) => {
|
||||
if let Err(err) = server.c_its_events.denm_event.notify(conn, &value).await {
|
||||
warn!("BLE: failed notifying connection of new DENM event: {err:?}");
|
||||
}
|
||||
}
|
||||
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:?}"))
|
||||
}
|
||||
360
src/io.rs
360
src/io.rs
|
|
@ -1,14 +1,16 @@
|
|||
//! UART and BLE I/O Protocol
|
||||
//! UART/ BLE I/O Protocol
|
||||
|
||||
pub mod msg {
|
||||
#![allow(clippy::all, clippy::pedantic, clippy::nursery, dead_code)]
|
||||
include!(concat!(env!("OUT_DIR"), "/c_its_io.rs"));
|
||||
}
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
use alloc::string::ToString;
|
||||
use alloc::vec::Vec;
|
||||
|
||||
use c_its_parser::gn as geonetworking;
|
||||
use log::error;
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
impl From<msg::RawMsgRx> for msg::SerialOutputMsg {
|
||||
fn from(value: msg::RawMsgRx) -> Self {
|
||||
use msg::serial_output_msg::Payload;
|
||||
|
|
@ -19,7 +21,6 @@ impl From<msg::RawMsgRx> for msg::SerialOutputMsg {
|
|||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
impl From<msg::DenmEvent> for msg::SerialOutputMsg {
|
||||
fn from(value: msg::DenmEvent) -> Self {
|
||||
use msg::serial_output_msg::Payload;
|
||||
|
|
@ -31,110 +32,22 @@ impl From<msg::DenmEvent> for msg::SerialOutputMsg {
|
|||
}
|
||||
|
||||
impl msg::RawMsgRx {
|
||||
#[cfg(feature = "uart")]
|
||||
pub fn serialize_uart_proto(&self) -> alloc::vec::Vec<u8> {
|
||||
pub fn serialize_uart_proto(&self) -> Vec<u8> {
|
||||
serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(self.clone()))
|
||||
}
|
||||
}
|
||||
|
||||
#[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> {
|
||||
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)
|
||||
}
|
||||
#[allow(unused)]
|
||||
pub fn serialize_uart_proto(&self) -> Vec<u8> {
|
||||
serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(*self))
|
||||
}
|
||||
}
|
||||
|
||||
#[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> {
|
||||
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)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
#[derive(Debug, Default)]
|
||||
pub struct Parser {
|
||||
buffer: alloc::vec::Vec<u8>,
|
||||
}
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
impl Parser {
|
||||
const SOF_LEN: u16 = 4;
|
||||
const SOF1: u8 = 0x56;
|
||||
|
|
@ -178,7 +91,7 @@ impl Parser {
|
|||
Ok(data) => {
|
||||
output.push(data);
|
||||
}
|
||||
Err(err) => log::error!("Failed to parse input: {err:?}"),
|
||||
Err(err) => error!("Failed to parse input: {err:?}"),
|
||||
}
|
||||
|
||||
// consume buffer
|
||||
|
|
@ -209,8 +122,7 @@ impl Parser {
|
|||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
fn serialize_serial_msg<T>(msg: &T) -> alloc::vec::Vec<u8>
|
||||
fn serialize_serial_msg<T>(msg: &T) -> Vec<u8>
|
||||
where
|
||||
T: prost::Message,
|
||||
{
|
||||
|
|
@ -229,7 +141,6 @@ where
|
|||
out
|
||||
}
|
||||
|
||||
#[cfg(any(feature = "uart", feature = "ble"))]
|
||||
impl From<msg::PositionState> for crate::applogic::GnssFix {
|
||||
fn from(value: msg::PositionState) -> Self {
|
||||
let time =
|
||||
|
|
@ -258,7 +169,6 @@ impl From<msg::PositionState> for crate::applogic::GnssFix {
|
|||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
impl From<msg::PositionState> for esp32_cits_core::PosVel {
|
||||
fn from(value: msg::PositionState) -> Self {
|
||||
let position = geo_types::Point::new(
|
||||
|
|
@ -284,7 +194,6 @@ impl From<msg::PositionState> for esp32_cits_core::PosVel {
|
|||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
impl From<msg::ItsPosition> for esp32_cits_core::PosVel {
|
||||
fn from(value: msg::ItsPosition) -> Self {
|
||||
let latitude_deg = f64::from(value.latitude) / 10_000_000.;
|
||||
|
|
@ -299,7 +208,6 @@ impl From<msg::ItsPosition> for esp32_cits_core::PosVel {
|
|||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx {
|
||||
fn make_eh(
|
||||
&self,
|
||||
|
|
@ -315,8 +223,6 @@ 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| {
|
||||
|
|
@ -390,20 +296,14 @@ impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx {
|
|||
|
||||
fn make_payload(
|
||||
&self,
|
||||
) -> Result<
|
||||
(
|
||||
alloc::vec::Vec<u8>,
|
||||
c_its_parser::standards::extensions::ItsMessageId,
|
||||
),
|
||||
alloc::string::String,
|
||||
> {
|
||||
) -> Result<(Vec<u8>, c_its_parser::standards::extensions::ItsMessageId), alloc::string::String>
|
||||
{
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
let message_id = (self.message_type as u8).try_into()?;
|
||||
Ok((self.payload.clone(), message_id))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
impl msg::RawMsgTx {
|
||||
fn make_geobcast(
|
||||
address: [u8; 6],
|
||||
|
|
@ -484,243 +384,12 @@ impl msg::RawMsgTx {
|
|||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "cam")]
|
||||
impl From<&alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::CAM>>
|
||||
for msg::CamEvent
|
||||
{
|
||||
fn from(
|
||||
value: &alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::CAM>,
|
||||
) -> Self {
|
||||
use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions;
|
||||
|
||||
let station_id = value.header.station_id.0;
|
||||
let station_type = u32::from(value.cam.cam_parameters.basic_container.station_type.0);
|
||||
let position = value
|
||||
.cam
|
||||
.cam_parameters
|
||||
.basic_container
|
||||
.reference_position
|
||||
.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,
|
||||
) => Some(basic_vehicle_container_low_frequency.vehicle_role as u32),
|
||||
_ => None,
|
||||
});
|
||||
|
||||
let vehicle_data = match &value.cam.cam_parameters.high_frequency_container {
|
||||
cam_pdu_descriptions::HighFrequencyContainer::basicVehicleContainerHighFrequency(
|
||||
basic_vehicle_container_high_frequency,
|
||||
) => Some(basic_vehicle_container_high_frequency.into()),
|
||||
_ => None,
|
||||
};
|
||||
|
||||
Self {
|
||||
station_id,
|
||||
station_type,
|
||||
position,
|
||||
vehicle_role,
|
||||
vehicle_data,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "cam")]
|
||||
impl From<&c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency> for msg::VehicleContainer {
|
||||
fn from(value: &c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency) -> Self {
|
||||
let heading_deg = if value.heading.heading_value.is_unavailable() {
|
||||
None
|
||||
} else {
|
||||
Some(u32::from(value.heading.heading_value.0))
|
||||
};
|
||||
let speed = if value.speed.speed_value.is_unavailable() {
|
||||
None
|
||||
} else {
|
||||
Some(u32::from(value.speed.speed_value.0))
|
||||
};
|
||||
let vehicle_length_dm = if value.vehicle_length.vehicle_length_value.is_unavailable() {
|
||||
None
|
||||
} else {
|
||||
Some(u32::from(value.vehicle_length.vehicle_length_value.0))
|
||||
};
|
||||
let vehicle_width_dm = if value.vehicle_width.is_unavailable() {
|
||||
None
|
||||
} else {
|
||||
Some(u32::from(value.vehicle_width.0))
|
||||
};
|
||||
|
||||
|
||||
Self { heading_deg, speed, vehicle_length_dm, vehicle_width_dm }
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "denm")]
|
||||
impl From<&alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_description::DENM>>
|
||||
for msg::DenmEvent
|
||||
{
|
||||
fn from(
|
||||
value: &alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_description::DENM>,
|
||||
) -> Self {
|
||||
let denm_mgmt = &value.denm.management;
|
||||
|
||||
let station_id = value.header.station_id.0; // TODO: or from action ID?
|
||||
|
||||
let ref_time = chrono::DateTime::<chrono::Utc>::from(denm_mgmt.reference_time.clone());
|
||||
let end_time = ref_time + chrono::Duration::seconds(denm_mgmt.validity_duration.0.into());
|
||||
|
||||
#[allow(clippy::cast_sign_loss)]
|
||||
let timestamp = ref_time.timestamp_millis() as u64;
|
||||
#[allow(clippy::cast_sign_loss)]
|
||||
let validity_end_ts = end_time.timestamp_millis() as u64;
|
||||
let seq_num = denm_mgmt.action_id.sequence_number.0.into();
|
||||
|
||||
let event_position = Some(denm_mgmt.event_position.clone().into());
|
||||
let event_heading_deg = value
|
||||
.denm
|
||||
.location
|
||||
.as_ref()
|
||||
.and_then(|v| v.event_position_heading.clone())
|
||||
.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
|
||||
.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());
|
||||
|
||||
Self {
|
||||
timestamp,
|
||||
validity_end_ts,
|
||||
station_id,
|
||||
seq_num,
|
||||
event_position,
|
||||
event_heading_deg,
|
||||
direction,
|
||||
cause_code,
|
||||
sub_cause_code,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "cam")]
|
||||
impl From<c_its_parser::standards::cdd_1_3_1_1::its_container::ReferencePosition>
|
||||
for msg::Position
|
||||
{
|
||||
fn from(value: c_its_parser::standards::cdd_1_3_1_1::its_container::ReferencePosition) -> Self {
|
||||
#[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
|
||||
let latitude_deg = value.latitude.as_deg() as f32;
|
||||
#[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
|
||||
let longitude_deg = value.longitude.as_deg() as f32;
|
||||
|
||||
Self {
|
||||
latitude_deg,
|
||||
longitude_deg,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "denm")]
|
||||
impl From<c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::ReferencePosition> for msg::Position {
|
||||
fn from(value: c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::ReferencePosition) -> Self {
|
||||
#[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
|
||||
let latitude_deg = value.latitude.as_deg() as f32;
|
||||
#[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
|
||||
let longitude_deg = value.longitude.as_deg() as f32;
|
||||
|
||||
Self {
|
||||
latitude_deg,
|
||||
longitude_deg,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "denm")]
|
||||
impl From<c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection> for msg::TrafficDir {
|
||||
fn from(value: c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection) -> Self {
|
||||
use c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection;
|
||||
|
||||
match value {
|
||||
TrafficDirection::allTrafficDirections => Self::AllDirections,
|
||||
TrafficDirection::sameAsReferenceDirection_upstreamOfReferencePosition => {
|
||||
Self::Upstream
|
||||
}
|
||||
TrafficDirection::sameAsReferenceDirection_downstreamOfReferencePosition => {
|
||||
Self::Downstream
|
||||
}
|
||||
TrafficDirection::oppositeToReferenceDirection => Self::Opposite,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(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 {
|
||||
|
||||
use super::*;
|
||||
use crate::init_test_env_logger;
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
impl From<msg::RawMsgTx> for msg::SerialInputMsg {
|
||||
fn from(value: msg::RawMsgTx) -> Self {
|
||||
use msg::serial_input_msg::Payload;
|
||||
|
|
@ -731,7 +400,6 @@ mod tests {
|
|||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
impl From<msg::PositionState> for msg::SerialInputMsg {
|
||||
fn from(value: msg::PositionState) -> Self {
|
||||
use msg::serial_input_msg::Payload;
|
||||
|
|
@ -745,9 +413,7 @@ mod tests {
|
|||
// manual example data:
|
||||
// SerialInputMsg(RawMsgTx): 5632582b0040123e0a340202c173256817284029d5b3400f1feaf01ffffffc23b7743e00000fc0007e8138250737feebfff600000dffff7ffff1ce40400010011807220028020d0a
|
||||
// SerialInputMsg(PositionState): 5632582b00170a1509a0bbe2a29f010000120a0d00002842150000b8410d0a
|
||||
// PositionState: 09a0bbe2a29f010000120a0d00002842150000b841
|
||||
|
||||
#[cfg(feature = "uart")]
|
||||
#[test]
|
||||
fn uart_parser() {
|
||||
// generate some test UART input messages
|
||||
|
|
|
|||
165
src/main.rs
165
src/main.rs
|
|
@ -18,9 +18,6 @@ compile_error!("feature \"cam_tx\" needs a time and position source (either GNSS
|
|||
// we can't have both GNSS drivers enabled at the same time
|
||||
#[cfg(all(feature = "gnss_ubx", feature = "gnss_l67k"))]
|
||||
compile_error!("feature \"gnss_ubx\" and feature \"gnss_l67k\" cannot be enabled at the same time");
|
||||
// at least one V2X feature needs to be enabled
|
||||
#[cfg(not(any(feature = "spat", feature = "cam", feature = "denm")))]
|
||||
compile_error!("at least one V2X feature (\"spat\", \"cam\" or \"denm\") needs to be enabled");
|
||||
|
||||
use alloc::string::ToString as _;
|
||||
use alloc::vec::Vec;
|
||||
|
|
@ -39,7 +36,7 @@ use embassy_gps::gps::l76k;
|
|||
use esp_backtrace as _;
|
||||
use esp_hal::clock::CpuClock;
|
||||
use esp_hal::timer::timg::TimerGroup;
|
||||
#[cfg(target_arch = "riscv32")]
|
||||
#[cfg(all(target_arch = "riscv32", feature = "spat"))]
|
||||
use esp_println::println;
|
||||
use esp32_cits_core::radio;
|
||||
use geonetworking::Decode as _;
|
||||
|
|
@ -48,11 +45,9 @@ use log::{error, info, warn};
|
|||
extern crate alloc;
|
||||
|
||||
mod applogic;
|
||||
#[cfg(feature = "ble")]
|
||||
mod ble;
|
||||
mod cache;
|
||||
mod geo_alg;
|
||||
#[cfg(any(feature = "uart", feature = "ble"))]
|
||||
#[cfg(feature = "uart")]
|
||||
mod io;
|
||||
#[cfg(feature = "screen")]
|
||||
mod screen;
|
||||
|
|
@ -68,9 +63,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
|
||||
|
||||
|
|
@ -78,26 +73,6 @@ const SERIAL_FIFO_SIZE_THLD: u8 = 100; // hardware has max. 128 byte
|
|||
// For more information see: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
|
||||
esp_bootloader_esp_idf::esp_app_desc!();
|
||||
|
||||
// Print a backtrace and reset (instead of halting)
|
||||
#[panic_handler]
|
||||
fn panic_handler(info: &core::panic::PanicInfo) -> ! {
|
||||
println!("");
|
||||
println!("====================== PANIC ======================");
|
||||
println!("{}", info);
|
||||
|
||||
println!("\nBacktrace:");
|
||||
let backtrace = esp_backtrace::Backtrace::capture();
|
||||
for frame in backtrace.frames() {
|
||||
println!("- 0x{:x}", frame.program_counter());
|
||||
}
|
||||
|
||||
println!("================== BACKTRACE END ==================");
|
||||
|
||||
// reboot
|
||||
println!("-> Doing a SW reset after panic...\n");
|
||||
esp_hal::system::software_reset();
|
||||
}
|
||||
|
||||
#[cfg(feature = "_gnss")]
|
||||
static GNSS_UPDATE: Mutex<RefCell<Option<applogic::GnssFix>>> = Mutex::new(RefCell::new(None));
|
||||
static WIFI_RX_QUEUE: Mutex<RefCell<Option<ArrayQueue<Vec<u8>>>>> = Mutex::new(RefCell::new(None));
|
||||
|
|
@ -114,7 +89,7 @@ static SERIAL: Mutex<RefCell<Option<esp_hal::uart::Uart<'static, esp_hal::Blocki
|
|||
#[esp_rtos::main]
|
||||
async fn main(spawner: Spawner) -> ! {
|
||||
// generator version: 1.3.0
|
||||
// generator parameters: --chip esp32c5 -o esp32c5-wroom-1-psram -o alloc -o log -o unstable-hal -o wifi -o esp-backtrace -o embassy -o ble-trouble
|
||||
// generator parameters: --chip esp32c5 -o esp32c5-wroom-1-psram -o alloc -o log -o unstable-hal -o wifi -o esp-backtrace -o embassy
|
||||
|
||||
esp_println::logger::init_logger_from_env();
|
||||
|
||||
|
|
@ -132,8 +107,6 @@ 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);
|
||||
|
|
@ -154,8 +127,8 @@ async fn main(spawner: Spawner) -> ! {
|
|||
let mut spi_buffer = [0_u8; 512];
|
||||
#[cfg(feature = "screen")]
|
||||
let mut screen = {
|
||||
// 240*280px in portrait orientation
|
||||
let (display, size) = screen::make_large_display(
|
||||
// 172*320px in landscape orientation
|
||||
let (display, width, height) = screen::make_small_display(
|
||||
peripherals.GPIO8,
|
||||
peripherals.GPIO10,
|
||||
peripherals.GPIO7,
|
||||
|
|
@ -167,7 +140,7 @@ async fn main(spawner: Spawner) -> ! {
|
|||
)
|
||||
.expect("Fatal error building screen");
|
||||
|
||||
let mut screen = screen::Handler::new(display, size);
|
||||
let mut screen = screen::Handler::new(display, width, height);
|
||||
|
||||
let _ = screen
|
||||
.update(screen::ScreenState::Initial)
|
||||
|
|
@ -215,20 +188,6 @@ async fn main(spawner: Spawner) -> ! {
|
|||
let mac = esp_hal::efuse::base_mac_address();
|
||||
info!("WiFi MAC: {mac}");
|
||||
|
||||
// Setup BLE
|
||||
#[cfg(feature = "ble")]
|
||||
{
|
||||
let controller = ble::make_controller(peripherals.BT);
|
||||
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"));
|
||||
}
|
||||
|
||||
// Setup GNSS:
|
||||
// Seeed XIAO L67K:
|
||||
// RX: D7/ GPIO12
|
||||
|
|
@ -379,22 +338,15 @@ async fn main(spawner: Spawner) -> ! {
|
|||
println!("{}", ubx::pretty_print_nav_sat(packet));
|
||||
}
|
||||
ublox::proto27::PacketRef::NavPvt(pvt) => {
|
||||
// error can be ignored b/c it's just when no GNSS fix yet
|
||||
if let Ok(pos_state) = applogic::GnssFix::try_from(pvt) {
|
||||
critical_section::with(|cs| {
|
||||
let gnss_update_ref = GNSS_UPDATE.borrow(cs);
|
||||
gnss_update_ref.replace(Some(pos_state));
|
||||
});
|
||||
match applogic::GnssFix::try_from(pvt) {
|
||||
Ok(pos_state) => {
|
||||
critical_section::with(|cs| {
|
||||
let gnss_update_ref = GNSS_UPDATE.borrow(cs);
|
||||
gnss_update_ref.replace(Some(pos_state));
|
||||
});
|
||||
}
|
||||
Err(err) => warn!("{err}"),
|
||||
}
|
||||
|
||||
// update GNSS HB on screen
|
||||
let _ = screen.update_gnss(pvt);
|
||||
}
|
||||
ublox::proto27::PacketRef::AckNak(nak) => {
|
||||
error!("UBX: {nak:?}");
|
||||
}
|
||||
ublox::proto27::PacketRef::AckAck(ack) => {
|
||||
info!("UBX: {ack:?}");
|
||||
}
|
||||
_ => {
|
||||
println!("UBX: {packet_ref:?}");
|
||||
|
|
@ -413,17 +365,6 @@ async fn main(spawner: Spawner) -> ! {
|
|||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "ble")]
|
||||
{
|
||||
critical_section::with(|cs| {
|
||||
let mut data_rc = ble::RX_DATA.borrow(cs).borrow_mut();
|
||||
if let Some(pos) = data_rc.take() {
|
||||
let gnss_update_ref = GNSS_UPDATE.borrow(cs);
|
||||
gnss_update_ref.replace(Some(pos.into()));
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
#[cfg(feature = "_gnss")]
|
||||
{
|
||||
let mut new_position = false;
|
||||
|
|
@ -474,9 +415,8 @@ async fn main(spawner: Spawner) -> ! {
|
|||
if let applogic::GlosaOutput::Locked(data) = &glosa_data {
|
||||
for sig in &data.signal_groups {
|
||||
let duration_str = sig
|
||||
.timing
|
||||
.as_ref()
|
||||
.map(|v| alloc::format!("for {} s", v.min_end_sec))
|
||||
.end_sec
|
||||
.map(|v| alloc::format!("for {v} s"))
|
||||
.unwrap_or_default();
|
||||
println!("GLOSA: {} {} {duration_str}", sig.maneuver, sig.phase);
|
||||
}
|
||||
|
|
@ -513,36 +453,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,49 +470,18 @@ async fn main(spawner: Spawner) -> ! {
|
|||
ItsMessage::Cam {
|
||||
geonetworking: _,
|
||||
transport: _,
|
||||
ref etsi,
|
||||
etsi,
|
||||
} => {
|
||||
#[cfg(feature = "cam")]
|
||||
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}"),
|
||||
}
|
||||
}
|
||||
}
|
||||
applogic::cam::handle_cam(&etsi);
|
||||
}
|
||||
|
||||
#[cfg(feature = "denm")]
|
||||
ItsMessage::DenmV2 {
|
||||
geonetworking: _,
|
||||
transport: _,
|
||||
ref 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}"),
|
||||
}
|
||||
}
|
||||
}
|
||||
etsi,
|
||||
} => applogic::denm::handle_denm(&etsi),
|
||||
#[cfg(feature = "denm")]
|
||||
ItsMessage::DenmV1 {
|
||||
geonetworking: _,
|
||||
|
|
@ -875,8 +764,6 @@ async fn gnss_ubx_config(reset_pin: esp_hal::peripherals::GPIO25<'static>) {
|
|||
cfg_data: &[
|
||||
ublox::cfg_val::CfgVal::Uart1OutProtNmea(false),
|
||||
ublox::cfg_val::CfgVal::Uart1OutProtUbx(true),
|
||||
// CFG-RATE-MEAS
|
||||
ublox::cfg_val::CfgVal::RateMeas(500),
|
||||
// CFG-MSGOUT-UBX_NAV_PVT_UART1
|
||||
ublox::cfg_val::CfgVal::MsgOutUbxNavPvtUart1(1),
|
||||
// CFG-MSGOUT-UBX_NAV_SAT_UART1
|
||||
|
|
|
|||
275
src/screen.rs
275
src/screen.rs
|
|
@ -5,24 +5,17 @@ use embedded_graphics::{geometry, mono_font, pixelcolor, primitives, text};
|
|||
|
||||
use crate::applogic;
|
||||
|
||||
pub struct DisplaySize {
|
||||
width: u16,
|
||||
height: u16,
|
||||
offset_top: i32,
|
||||
corner_radius: i32,
|
||||
}
|
||||
|
||||
pub struct Handler<D>
|
||||
where
|
||||
D: DrawTarget<Color = pixelcolor::Rgb565>,
|
||||
{
|
||||
target: D,
|
||||
size: DisplaySize,
|
||||
width: u16,
|
||||
height: u16,
|
||||
|
||||
// internal state
|
||||
prev_state: ScreenState,
|
||||
hb_state: bool,
|
||||
gnss_hb_state: bool,
|
||||
}
|
||||
|
||||
#[derive(Debug, Default, PartialEq, Eq)]
|
||||
|
|
@ -86,20 +79,16 @@ where
|
|||
|
||||
const DEFAULT_TEXT_STYLE: mono_font::MonoTextStyle<'static, pixelcolor::Rgb565> =
|
||||
mono_font::MonoTextStyle::new(&profont::PROFONT_24_POINT, Self::COLOR_FG);
|
||||
const MID_TEXT_STYLE: mono_font::MonoTextStyle<'static, pixelcolor::Rgb565> =
|
||||
mono_font::MonoTextStyle::new(&profont::PROFONT_18_POINT, Self::COLOR_FG);
|
||||
const SMALL_TEXT_STYLE: mono_font::MonoTextStyle<'static, pixelcolor::Rgb565> =
|
||||
mono_font::MonoTextStyle::new(&profont::PROFONT_14_POINT, Self::COLOR_FG);
|
||||
|
||||
const BIG_HEIGHT_THLD: u16 = 172;
|
||||
|
||||
pub fn new(target: D, display_size: DisplaySize) -> Self {
|
||||
pub fn new(target: D, width: u16, height: u16) -> Self {
|
||||
Self {
|
||||
target,
|
||||
size: display_size,
|
||||
width,
|
||||
height,
|
||||
prev_state: ScreenState::Uninitialized,
|
||||
hb_state: false,
|
||||
gnss_hb_state: false,
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -126,19 +115,18 @@ where
|
|||
// Update indicator/ heartbeat
|
||||
{
|
||||
let update_ind_dia = 10;
|
||||
let circle_x_pos =
|
||||
i32::from(self.size.width) - update_ind_dia - self.size.corner_radius;
|
||||
let circle_x_pos = i32::from(self.width) - update_ind_dia - 10;
|
||||
|
||||
text::Text::with_alignment(
|
||||
"HB",
|
||||
geometry::Point::new(circle_x_pos - 5, 10 + self.size.offset_top),
|
||||
geometry::Point::new(circle_x_pos - 5, 10),
|
||||
Self::SMALL_TEXT_STYLE,
|
||||
text::Alignment::Right,
|
||||
)
|
||||
.draw(&mut self.target)?;
|
||||
|
||||
primitives::Circle::new(
|
||||
geometry::Point::new(circle_x_pos, 2 + self.size.offset_top),
|
||||
geometry::Point::new(circle_x_pos, 2),
|
||||
update_ind_dia.cast_unsigned(),
|
||||
)
|
||||
.into_styled(if self.hb_state {
|
||||
|
|
@ -155,67 +143,13 @@ where
|
|||
Ok(())
|
||||
}
|
||||
|
||||
// on big screns: uses full screen width and height from 250 to 280 pixels
|
||||
pub fn update_gnss(
|
||||
&mut self,
|
||||
pvt: &ublox::nav_pvt::proto27::NavPvtRef,
|
||||
) -> Result<(), D::Error> {
|
||||
use alloc::string::ToString as _;
|
||||
|
||||
// this only applies to big screens
|
||||
if !self.is_big_height() {
|
||||
return Ok(());
|
||||
}
|
||||
|
||||
let num_sat = pvt.num_satellites();
|
||||
let fix_str = match pvt.fix_type() {
|
||||
ublox::GnssFixType::NoFix
|
||||
| ublox::GnssFixType::DeadReckoningOnly
|
||||
| ublox::GnssFixType::TimeOnlyFix => {
|
||||
alloc::format!("{num_sat:2} sat")
|
||||
}
|
||||
ublox::GnssFixType::Fix2D => "2D fix".to_string(),
|
||||
ublox::GnssFixType::Fix3D | ublox::GnssFixType::GPSPlusDeadReckoning => {
|
||||
"3D fix".to_string()
|
||||
}
|
||||
_ => " ".to_string(),
|
||||
};
|
||||
|
||||
// clear area and write information
|
||||
// note: text position is x baseline, not top left corner
|
||||
primitives::Rectangle::new(
|
||||
geometry::Point::new(0, 250 + self.size.offset_top),
|
||||
geometry::Size::new(self.size.width.into(), 20),
|
||||
)
|
||||
.into_styled(Self::BK_STYLE)
|
||||
.draw(&mut self.target)?;
|
||||
|
||||
let colon = if self.gnss_hb_state { ":" } else { " " };
|
||||
let message = alloc::format!(
|
||||
"{fix_str}{colon} {:2.0}km/h, {:3.0}°",
|
||||
pvt.ground_speed_2d() * 3.6,
|
||||
pvt.heading_motion()
|
||||
);
|
||||
text::Text::with_alignment(
|
||||
&message,
|
||||
geometry::Point::new(self.size.corner_radius / 2, 265 + self.size.offset_top),
|
||||
Self::SMALL_TEXT_STYLE,
|
||||
text::Alignment::Left,
|
||||
)
|
||||
.draw(&mut self.target)?;
|
||||
|
||||
self.gnss_hb_state = !self.gnss_hb_state;
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn clear(&mut self) -> Result<(), D::Error>
|
||||
where
|
||||
D: DrawTarget<Color = pixelcolor::Rgb565>,
|
||||
{
|
||||
primitives::Rectangle::new(
|
||||
geometry::Point::new(0, 0),
|
||||
geometry::Size::new(self.size.width.into(), self.size.height.into()),
|
||||
geometry::Size::new(self.width.into(), self.height.into()),
|
||||
)
|
||||
.into_styled(Self::BK_STYLE)
|
||||
.draw(&mut self.target)?;
|
||||
|
|
@ -223,10 +157,6 @@ where
|
|||
Ok(())
|
||||
}
|
||||
|
||||
fn is_big_height(&self) -> bool {
|
||||
self.size.height > Self::BIG_HEIGHT_THLD
|
||||
}
|
||||
|
||||
#[allow(unused)]
|
||||
pub fn test_img(&mut self) -> Result<(), D::Error> {
|
||||
mipidsi::TestImage::new().draw(&mut self.target)?;
|
||||
|
|
@ -241,7 +171,7 @@ where
|
|||
|
||||
text::Text::with_alignment(
|
||||
msg,
|
||||
geometry::Point::new(i32::from(self.size.width) / 2, 40 + self.size.offset_top),
|
||||
geometry::Point::new(i32::from(self.width) / 2, 40),
|
||||
Self::DEFAULT_TEXT_STYLE,
|
||||
text::Alignment::Center,
|
||||
)
|
||||
|
|
@ -256,8 +186,8 @@ where
|
|||
self.clear()?;
|
||||
|
||||
text::Text::with_alignment(
|
||||
"Searching...",
|
||||
geometry::Point::new(i32::from(self.size.width) / 2, 130 + self.size.offset_top),
|
||||
"No upcoming SPAT",
|
||||
geometry::Point::new(i32::from(self.width) / 2, 130),
|
||||
Self::DEFAULT_TEXT_STYLE,
|
||||
text::Alignment::Center,
|
||||
)
|
||||
|
|
@ -279,7 +209,7 @@ where
|
|||
|
||||
text::Text::with_alignment(
|
||||
&text,
|
||||
geometry::Point::new(i32::from(self.size.width) / 2, 130 + self.size.offset_top),
|
||||
geometry::Point::new(i32::from(self.width) / 2, 130),
|
||||
Self::DEFAULT_TEXT_STYLE,
|
||||
text::Alignment::Center,
|
||||
)
|
||||
|
|
@ -288,9 +218,6 @@ where
|
|||
Ok(())
|
||||
}
|
||||
|
||||
// uses full screen width and height
|
||||
// - from 0 to 105+67 = 172 pixels for small screens
|
||||
// - from 0 to 105+67+20 = 192 pixels for big screens
|
||||
#[allow(unused)]
|
||||
fn draw_glosa(
|
||||
&mut self,
|
||||
|
|
@ -299,8 +226,6 @@ where
|
|||
) -> Result<(), D::Error> {
|
||||
let num_signals = data.signal_groups.len();
|
||||
|
||||
let is_big_height = self.is_big_height();
|
||||
|
||||
if let Some(prev) = prev_data
|
||||
&& prev.signal_groups.len() != num_signals
|
||||
{
|
||||
|
|
@ -308,30 +233,25 @@ where
|
|||
self.clear();
|
||||
} else {
|
||||
// only clear maneuver and timing area otherwise
|
||||
let sig_total_height = 67 + if is_big_height { 20 } else { 0 };
|
||||
|
||||
primitives::Rectangle::new(
|
||||
geometry::Point::new(0, 105 + self.size.offset_top),
|
||||
geometry::Size::new(self.size.width.into(), sig_total_height),
|
||||
geometry::Point::new(0, 105),
|
||||
geometry::Size::new(self.width.into(), 67),
|
||||
)
|
||||
.into_styled(Self::BK_STYLE)
|
||||
.draw(&mut self.target)?;
|
||||
}
|
||||
|
||||
// do nothing more when no signals
|
||||
if num_signals == 0 {
|
||||
return Ok(());
|
||||
}
|
||||
|
||||
// draw intersection ID
|
||||
text::Text::new(
|
||||
&alloc::format!("#{}", data.intersection_id),
|
||||
geometry::Point::new(self.size.corner_radius + 5, 10 + self.size.offset_top),
|
||||
geometry::Point::new(15, 10),
|
||||
Self::SMALL_TEXT_STYLE,
|
||||
)
|
||||
.draw(&mut self.target)?;
|
||||
|
||||
#[allow(clippy::cast_possible_truncation, clippy::cast_possible_wrap)]
|
||||
let sig_width = i32::from(self.size.width) / (num_signals as i32);
|
||||
let sig_width = i32::from(self.width) / (num_signals as i32);
|
||||
|
||||
for (idx, sig) in data.signal_groups.iter().enumerate() {
|
||||
#[allow(clippy::cast_possible_truncation, clippy::cast_possible_wrap)]
|
||||
|
|
@ -349,7 +269,7 @@ where
|
|||
applogic::SignalPhase::RedYellow | applogic::SignalPhase::Yellow => Self::YE_STYLE,
|
||||
};
|
||||
primitives::Circle::new(
|
||||
geometry::Point::new(offset + circle_margin, 15 + self.size.offset_top),
|
||||
geometry::Point::new(offset + circle_margin, 15),
|
||||
circle_dia.cast_unsigned(),
|
||||
)
|
||||
.into_styled(style)
|
||||
|
|
@ -360,50 +280,24 @@ where
|
|||
// draw maneuver
|
||||
text::Text::with_alignment(
|
||||
&maneuver_to_str(sig.maneuver),
|
||||
geometry::Point::new(centerline, 130 + self.size.offset_top),
|
||||
geometry::Point::new(centerline, 130),
|
||||
Self::DEFAULT_TEXT_STYLE,
|
||||
text::Alignment::Center,
|
||||
)
|
||||
.draw(&mut self.target)?;
|
||||
|
||||
// draw timing indicator(s)
|
||||
if let Some(timing) = &sig.timing {
|
||||
if let (Some(likely), Some(max)) = (timing.likely_end_sec, timing.max_end_sec) {
|
||||
text::Text::with_alignment(
|
||||
&alloc::format!("{likely}s"),
|
||||
geometry::Point::new(centerline, 160 + self.size.offset_top),
|
||||
Self::DEFAULT_TEXT_STYLE,
|
||||
text::Alignment::Center,
|
||||
)
|
||||
.draw(&mut self.target)?;
|
||||
|
||||
// only for big screen
|
||||
if is_big_height {
|
||||
// TODO: show as diff to likely time?
|
||||
text::Text::with_alignment(
|
||||
&alloc::format!("{}-{max}", timing.min_end_sec),
|
||||
geometry::Point::new(centerline, 185 + self.size.offset_top),
|
||||
Self::MID_TEXT_STYLE,
|
||||
text::Alignment::Center,
|
||||
)
|
||||
.draw(&mut self.target)?;
|
||||
}
|
||||
} else {
|
||||
text::Text::with_alignment(
|
||||
&alloc::format!("{}s", timing.min_end_sec),
|
||||
geometry::Point::new(centerline, 160 + self.size.offset_top),
|
||||
Self::DEFAULT_TEXT_STYLE,
|
||||
text::Alignment::Center,
|
||||
)
|
||||
.draw(&mut self.target)?;
|
||||
}
|
||||
// draw timing indicator
|
||||
if let Some(end_sec) = sig.end_sec {
|
||||
text::Text::with_alignment(
|
||||
&alloc::format!("{end_sec}s"),
|
||||
geometry::Point::new(centerline, 160),
|
||||
Self::DEFAULT_TEXT_STYLE,
|
||||
text::Alignment::Center,
|
||||
)
|
||||
.draw(&mut self.target)?;
|
||||
}
|
||||
}
|
||||
|
||||
// if is_big_height {
|
||||
// // TODO: Add ETA
|
||||
// }
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
|
|
@ -413,7 +307,7 @@ where
|
|||
|
||||
self.clear()?;
|
||||
|
||||
let sig_width = i32::from(self.size.width) / 3;
|
||||
let sig_width = i32::from(self.width) / 3;
|
||||
let circle_dia = 50;
|
||||
let circle_margin = (sig_width - circle_dia) / 2;
|
||||
|
||||
|
|
@ -425,7 +319,7 @@ where
|
|||
let offset = (idx as i32) * sig_width;
|
||||
|
||||
primitives::Circle::new(
|
||||
geometry::Point::new(offset + circle_margin, 15 + self.size.offset_top),
|
||||
geometry::Point::new(offset + circle_margin, 15),
|
||||
circle_dia.cast_unsigned(),
|
||||
)
|
||||
.into_styled(*style)
|
||||
|
|
@ -433,10 +327,10 @@ where
|
|||
}
|
||||
|
||||
// draw welcome text
|
||||
let centerline = i32::from(self.size.width) / 2;
|
||||
let centerline = i32::from(self.width) / 2;
|
||||
text::Text::with_alignment(
|
||||
"C-ITS Signal Phase\nWaiting for GNSS...",
|
||||
geometry::Point::new(centerline, 130 + self.size.offset_top),
|
||||
"C-ITS Signal Phase Display\nWaiting for GNSS fix...",
|
||||
geometry::Point::new(centerline, 130),
|
||||
text_style,
|
||||
text::Alignment::Center,
|
||||
)
|
||||
|
|
@ -459,7 +353,7 @@ fn maneuver_to_str(value: applogic::v2x::map::Maneuvers) -> alloc::string::Strin
|
|||
///
|
||||
/// # Errors
|
||||
/// Human-readable fatal errors
|
||||
#[allow(unused, clippy::too_many_arguments, clippy::type_complexity)]
|
||||
#[allow(clippy::too_many_arguments, clippy::type_complexity)]
|
||||
pub fn make_small_display<IoScl, IoSda, IoRes, IoDc, IoCs, IoBl, Spi>(
|
||||
scl: IoScl,
|
||||
sda: IoSda,
|
||||
|
|
@ -484,7 +378,8 @@ pub fn make_small_display<IoScl, IoSda, IoRes, IoDc, IoCs, IoBl, Spi>(
|
|||
mipidsi::models::ST7789,
|
||||
esp_hal::gpio::Output<'_>,
|
||||
>,
|
||||
DisplaySize,
|
||||
u16,
|
||||
u16,
|
||||
),
|
||||
alloc::string::String,
|
||||
>
|
||||
|
|
@ -501,15 +396,10 @@ where
|
|||
esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static,
|
||||
Spi: esp_hal::spi::master::Instance + 'static,
|
||||
{
|
||||
const DISPLAY_WIDTH: u16 = 320;
|
||||
const DISPLAY_HEIGHT: u16 = 172;
|
||||
const DISPLAY_HIDDEN_X: u16 = 34; // somehow this display has 34 px of invisible space on the left (in native orientation)
|
||||
|
||||
let size = DisplaySize {
|
||||
width: 320,
|
||||
height: 172,
|
||||
offset_top: 0,
|
||||
corner_radius: 10,
|
||||
};
|
||||
|
||||
let (spi, cs) = setup_st7789_spi(scl, sda, cs, spi)
|
||||
.map_err(|err| alloc::format!("Failed to build SPI config: {err}"))?;
|
||||
|
||||
|
|
@ -523,101 +413,18 @@ where
|
|||
let spi_device = embedded_hal_bus::spi::ExclusiveDevice::new_no_delay(spi, cs)
|
||||
.map_err(|err| alloc::format!("Failed to initialize SPI device: {err}"))?;
|
||||
|
||||
// Note: height and width are swapped b/c of 90° display rotation
|
||||
let display = mipidsi::Builder::new(
|
||||
mipidsi::models::ST7789,
|
||||
mipidsi::interface::SpiInterface::new(spi_device, dc_output, spi_buffer),
|
||||
)
|
||||
.display_size(size.height + DISPLAY_HIDDEN_X, size.width)
|
||||
.display_size(DISPLAY_HEIGHT + DISPLAY_HIDDEN_X, DISPLAY_WIDTH)
|
||||
.reset_pin(rst_output)
|
||||
.invert_colors(mipidsi::options::ColorInversion::Inverted)
|
||||
.orientation(mipidsi::options::Orientation::new().rotate(mipidsi::options::Rotation::Deg90))
|
||||
.init(&mut display_delay)
|
||||
.map_err(|err| alloc::format!("Failed to initialize display: {err:?}"))?;
|
||||
|
||||
Ok((display, size))
|
||||
}
|
||||
|
||||
/// Creates an [`mipidsi::Display`], width, height tuple for an ST7789 240*280px screen in portrait orientation
|
||||
///
|
||||
/// # Errors
|
||||
/// Human-readable fatal errors
|
||||
#[allow(unused, clippy::too_many_arguments, clippy::type_complexity)]
|
||||
pub fn make_large_display<IoScl, IoSda, IoRes, IoDc, IoCs, IoBl, Spi>(
|
||||
scl: IoScl,
|
||||
sda: IoSda,
|
||||
res: IoRes,
|
||||
dc: IoDc,
|
||||
cs: IoCs,
|
||||
bl: IoBl,
|
||||
spi: Spi,
|
||||
spi_buffer: &mut [u8],
|
||||
) -> Result<
|
||||
(
|
||||
mipidsi::Display<
|
||||
mipidsi::interface::SpiInterface<
|
||||
'_,
|
||||
embedded_hal_bus::spi::ExclusiveDevice<
|
||||
esp_hal::spi::master::Spi<'_, esp_hal::Blocking>,
|
||||
esp_hal::gpio::Output<'_>,
|
||||
embedded_hal_bus::spi::NoDelay,
|
||||
>,
|
||||
esp_hal::gpio::Output<'_>,
|
||||
>,
|
||||
mipidsi::models::ST7789,
|
||||
esp_hal::gpio::Output<'_>,
|
||||
>,
|
||||
DisplaySize,
|
||||
),
|
||||
alloc::string::String,
|
||||
>
|
||||
where
|
||||
IoScl: esp_hal::gpio::interconnect::PeripheralOutput<'static>,
|
||||
IoSda: esp_hal::gpio::interconnect::PeripheralOutput<'static>,
|
||||
IoRes:
|
||||
esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static,
|
||||
IoDc:
|
||||
esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static,
|
||||
IoCs:
|
||||
esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static,
|
||||
IoBl:
|
||||
esp_hal::gpio::interconnect::PeripheralOutput<'static> + esp_hal::gpio::OutputPin + 'static,
|
||||
Spi: esp_hal::spi::master::Instance + 'static,
|
||||
{
|
||||
const DISPLAY_HIDDEN_Y: u16 = 20; // somehow this display has 20 px of invisible space on the top (in native orientation)
|
||||
|
||||
let size = DisplaySize {
|
||||
width: 240,
|
||||
height: 280 + DISPLAY_HIDDEN_Y,
|
||||
offset_top: DISPLAY_HIDDEN_Y.into(),
|
||||
corner_radius: 30,
|
||||
};
|
||||
|
||||
let (spi, cs) = setup_st7789_spi(scl, sda, cs, spi)
|
||||
.map_err(|err| alloc::format!("Failed to build SPI config: {err}"))?;
|
||||
|
||||
let output_config = esp_hal::gpio::OutputConfig::default();
|
||||
let dc_output = esp_hal::gpio::Output::new(dc, esp_hal::gpio::Level::Low, output_config);
|
||||
let rst_output = esp_hal::gpio::Output::new(res, esp_hal::gpio::Level::High, output_config);
|
||||
let _ = esp_hal::gpio::Output::new(bl, esp_hal::gpio::Level::High, output_config); // enable backlight
|
||||
|
||||
let mut display_delay = esp_hal::delay::Delay::new();
|
||||
|
||||
let spi_device = embedded_hal_bus::spi::ExclusiveDevice::new_no_delay(spi, cs)
|
||||
.map_err(|err| alloc::format!("Failed to initialize SPI device: {err}"))?;
|
||||
|
||||
let display = mipidsi::Builder::new(
|
||||
mipidsi::models::ST7789,
|
||||
mipidsi::interface::SpiInterface::new(spi_device, dc_output, spi_buffer),
|
||||
)
|
||||
.display_size(size.width, size.height + DISPLAY_HIDDEN_Y)
|
||||
.reset_pin(rst_output)
|
||||
.invert_colors(mipidsi::options::ColorInversion::Inverted)
|
||||
.orientation(mipidsi::options::Orientation::new().rotate(mipidsi::options::Rotation::Deg0))
|
||||
.init(&mut display_delay)
|
||||
.map_err(|err| alloc::format!("Failed to initialize display: {err:?}"))?;
|
||||
|
||||
Ok((display, size))
|
||||
Ok((display, DISPLAY_WIDTH, DISPLAY_HEIGHT))
|
||||
}
|
||||
|
||||
/// Builds the SPI interface for the ST7789 display
|
||||
|
|
|
|||
96
src/test-proto.rs
Normal file
96
src/test-proto.rs
Normal file
|
|
@ -0,0 +1,96 @@
|
|||
#![cfg(not(target_arch = "riscv32"))]
|
||||
#![allow(unused)]
|
||||
|
||||
extern crate alloc;
|
||||
|
||||
mod applogic;
|
||||
mod io;
|
||||
|
||||
use esp32_cits_core::tx;
|
||||
|
||||
fn main() {
|
||||
use prost::Message as _;
|
||||
|
||||
// env_logger::init();
|
||||
|
||||
// generate some test UART messages as hex strings
|
||||
let raw_its = input_from_raw(io::msg::RawMsgTx {
|
||||
payload: [0xaa, 0xbb, 0xcc, 0xdd].to_vec(),
|
||||
message_type: 1,
|
||||
gn_transport: io::msg::GnTransport::Shb.into(),
|
||||
gn_area: io::msg::GnArea {
|
||||
area: None,
|
||||
position: None,
|
||||
dist_a: None,
|
||||
dist_b: None,
|
||||
angle: None,
|
||||
},
|
||||
hop_limit: 2,
|
||||
station_type: None,
|
||||
});
|
||||
println!("{}", serialize_serial_msg_hex(&raw_its));
|
||||
|
||||
let pos = io::msg::PositionState {
|
||||
timestamp_ms: 1785144196000,
|
||||
position: io::msg::Position {
|
||||
latitude_deg: 42.,
|
||||
longitude_deg: 23.,
|
||||
},
|
||||
heading: None,
|
||||
speed: None,
|
||||
};
|
||||
println!("{}", serialize_serial_msg_hex(&input_from_position(pos)));
|
||||
|
||||
// generate test BLE message as hex strings
|
||||
println!("{}", buf_to_hex(&pos.encode_to_vec()));
|
||||
}
|
||||
|
||||
fn input_from_raw(value: io::msg::RawMsgTx) -> io::msg::SerialInputMsg {
|
||||
use io::msg::serial_input_msg::Payload;
|
||||
|
||||
io::msg::SerialInputMsg {
|
||||
payload: Some(Payload::TxMsg(value)),
|
||||
}
|
||||
}
|
||||
fn input_from_position(value: io::msg::PositionState) -> io::msg::SerialInputMsg {
|
||||
use io::msg::serial_input_msg::Payload;
|
||||
|
||||
io::msg::SerialInputMsg {
|
||||
payload: Some(Payload::Position(value)),
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn serialize_serial_msg_hex(msg: &io::msg::SerialInputMsg) -> alloc::string::String {
|
||||
use prost::Message as _;
|
||||
|
||||
let mut msg_buf = msg.encode_to_vec();
|
||||
|
||||
let sof = [0x56u8, 0x32, 0x58, 0x2B];
|
||||
let eof = [0x0du8, 0x0a];
|
||||
let len: [u8; 2] = (msg_buf.len() as u16).to_be_bytes();
|
||||
|
||||
let mut out = sof.to_vec();
|
||||
out.extend_from_slice(&len);
|
||||
out.append(&mut msg_buf);
|
||||
out.extend_from_slice(&eof);
|
||||
|
||||
buf_to_hex(&out)
|
||||
}
|
||||
|
||||
pub(crate) fn buf_to_hex(data: &[u8]) -> alloc::string::String {
|
||||
let mut hex_str = alloc::string::String::default();
|
||||
for c in data {
|
||||
hex_str.push_str(format!("{c:02x}").as_str());
|
||||
}
|
||||
|
||||
hex_str
|
||||
}
|
||||
|
||||
// /// Initializes the env-logger to be used inside tests
|
||||
// fn init_test_env_logger() {
|
||||
// let _ = env_logger::builder()
|
||||
// .format_timestamp(None)
|
||||
// .filter_level(log::LevelFilter::Debug)
|
||||
// .is_test(true)
|
||||
// .try_init();
|
||||
// }
|
||||
Loading…
Reference in a new issue