Move core ESP32 V2X to own repo
This commit is contained in:
Jannik Beyerstedt
parent
2db0dc0116
commit
666499c240
11 changed files with 118 additions and 615 deletions
17
Cargo.lock
generated
17
Cargo.lock
generated
|
|
@ -192,9 +192,9 @@ checksum = "1e748733b7cbc798e1434b6ac524f0c1ff2ab456fe201501e6497c8417a4fc33"
|
|||
|
||||
[[package]]
|
||||
name = "c-its-parser"
|
||||
version = "2.4.0"
|
||||
version = "2.4.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "cd4db66f495280ec88eef8712dd9ab7d1aacc590f62485e3e03a89b3ce4f8b14"
|
||||
checksum = "7e94d9990b20498f621f164f23ae198c4365115eb4191ae106352ec74d4a4110"
|
||||
dependencies = [
|
||||
"chrono",
|
||||
"etherparse",
|
||||
|
|
@ -1171,6 +1171,7 @@ dependencies = [
|
|||
"esp-println",
|
||||
"esp-radio",
|
||||
"esp-rtos",
|
||||
"esp32-cits-core",
|
||||
"geo-types",
|
||||
"log",
|
||||
"mipidsi",
|
||||
|
|
@ -1181,6 +1182,18 @@ dependencies = [
|
|||
"ublox",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "esp32-cits-core"
|
||||
version = "0.1.0"
|
||||
source = "git+https://git.hamburg.ccc.de/jtbx/esp32-cits-core?branch=main#a6d0ea6ca2da130fde2b84060a34ef45e772a459"
|
||||
dependencies = [
|
||||
"c-its-parser",
|
||||
"chrono",
|
||||
"esp-hal",
|
||||
"esp-radio",
|
||||
"geo-types",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "esp32c2"
|
||||
version = "0.29.2"
|
||||
|
|
|
|||
|
|
@ -30,8 +30,10 @@ esp = [
|
|||
"dep:esp-backtrace",
|
||||
"dep:esp-println",
|
||||
"dep:esp-radio",
|
||||
"esp32-cits-core/esp32c5",
|
||||
"esp32-cits-core/log-04",
|
||||
]
|
||||
std = ["c-its-parser/std"]
|
||||
std = ["c-its-parser/std", "esp32-cits-core/std"]
|
||||
|
||||
# enable u-blox GNSS module
|
||||
gnss_ubx = ["_gnss", "_uart", "dep:ublox"]
|
||||
|
|
@ -93,6 +95,7 @@ esp-radio = { version = "0.18.0", optional = true, features = [
|
|||
] }
|
||||
|
||||
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 }
|
||||
# needs newer GN with removed bitvec dependency
|
||||
|
|
|
|||
|
|
@ -11,6 +11,45 @@ const PH_MAX_LENGHT_M: f32 = 500.;
|
|||
const PH_MAX_ITEMS: usize = 23;
|
||||
const PH_MIN_DIST_M: f32 = 20.;
|
||||
|
||||
/// Stand-alone CAM generator
|
||||
///
|
||||
/// Generates CAM messages from time and [`PosVel`] updates
|
||||
///
|
||||
/// # Send a message
|
||||
/// ```
|
||||
/// use esp32_cits_core::tx::GnItsPayload as _;
|
||||
///
|
||||
/// let (mut seq_no, mac_bytes) = {
|
||||
/// // unwrap is fine since [`esp_hal::efuse::MacAddress`] is just an `[u8; 6]`
|
||||
/// let mac_bytes = mac.as_bytes().try_into().unwrap();
|
||||
///
|
||||
/// (0, mac_bytes)
|
||||
/// };
|
||||
///
|
||||
/// let mut cam = {
|
||||
/// let station_id = make_station_id(mac);
|
||||
/// info!("V2X Station ID: {station_id}");
|
||||
///
|
||||
/// applogic::cam_tx::CamState::new(
|
||||
/// station_id,
|
||||
/// OWN_STATION_TYPE,
|
||||
/// OWN_VEHICLE_LENGTH,
|
||||
/// OWN_VEHICLE_WIDTH,
|
||||
/// )
|
||||
/// };
|
||||
///
|
||||
/// cam.update(time, fix.clone().into());
|
||||
/// match cam.encode_packet(mac_bytes, pos_vel, time, GN_IS_MOBILE, &mut seq_no) {
|
||||
/// Ok(data) => {
|
||||
/// if let Err(err) =
|
||||
/// radio::send_80211_bcast_frame(&mut wlan_iface, &mac_bytes.clone(), &data)
|
||||
/// {
|
||||
/// warn!("Failed to send 802.11 frame: {err}");
|
||||
/// }
|
||||
/// }
|
||||
/// Err(err) => warn!("Failed to create CAM: {err}"),
|
||||
/// }
|
||||
/// ```
|
||||
#[derive(Debug)]
|
||||
pub struct CamState {
|
||||
station_id: u32,
|
||||
|
|
@ -20,7 +59,7 @@ pub struct CamState {
|
|||
vehicle_width_dm: u8, // vehicle width is in 10cm steps
|
||||
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
pos_state: super::PositionState,
|
||||
pos_state: esp32_cits_core::PosVel,
|
||||
|
||||
/// database for path history, oldest item at the back of the vector
|
||||
path_history: Vec<PathPoint>,
|
||||
|
|
@ -35,7 +74,7 @@ struct PathPoint {
|
|||
}
|
||||
|
||||
impl PathPoint {
|
||||
fn new_from_pos(pos: &super::PositionState, time: chrono::DateTime<chrono::Utc>) -> Self {
|
||||
fn new_from_pos(pos: &esp32_cits_core::PosVel, time: chrono::DateTime<chrono::Utc>) -> Self {
|
||||
Self {
|
||||
pos: pos.position,
|
||||
time,
|
||||
|
|
@ -44,7 +83,7 @@ impl PathPoint {
|
|||
}
|
||||
|
||||
fn new_from_pos_with_dist(
|
||||
pos: &super::PositionState,
|
||||
pos: &esp32_cits_core::PosVel,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
dist_m: f32,
|
||||
) -> Self {
|
||||
|
|
@ -69,12 +108,12 @@ impl CamState {
|
|||
vehicle_length_dm,
|
||||
vehicle_width_dm,
|
||||
time: chrono::DateTime::<chrono::Utc>::default(),
|
||||
pos_state: super::PositionState::default(),
|
||||
pos_state: esp32_cits_core::PosVel::default(),
|
||||
path_history: Vec::with_capacity(PH_MAX_ITEMS + 1),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn update(&mut self, time: chrono::DateTime<chrono::Utc>, pos: super::PositionState) {
|
||||
pub fn update(&mut self, time: chrono::DateTime<chrono::Utc>, pos: esp32_cits_core::PosVel) {
|
||||
self.time = time;
|
||||
self.pos_state = pos;
|
||||
|
||||
|
|
@ -86,7 +125,7 @@ impl CamState {
|
|||
fn update_ph(
|
||||
path_history: &mut Vec<PathPoint>,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
pos: &super::PositionState,
|
||||
pos: &esp32_cits_core::PosVel,
|
||||
) {
|
||||
// Update path history
|
||||
if path_history.is_empty() {
|
||||
|
|
@ -293,11 +332,11 @@ impl CamState {
|
|||
}
|
||||
}
|
||||
|
||||
impl crate::v2x_tx::GnItsPayload for CamState {
|
||||
impl esp32_cits_core::tx::GnItsPayload for CamState {
|
||||
fn make_eh(
|
||||
&self,
|
||||
address: [u8; 6],
|
||||
own_position: &crate::applogic::PositionState,
|
||||
own_position: &esp32_cits_core::PosVel,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
_seq_no: &mut u16,
|
||||
) -> Result<
|
||||
|
|
@ -313,7 +352,7 @@ impl crate::v2x_tx::GnItsPayload for CamState {
|
|||
|
||||
// CAM always uses SHB, hop-limit 1
|
||||
Ok((
|
||||
crate::v2x_tx::gn::make_shb_eh(address, station_type, own_position, time),
|
||||
esp32_cits_core::tx::gn::make_shb_eh(address, station_type, own_position, time)?,
|
||||
None,
|
||||
1,
|
||||
))
|
||||
|
|
@ -336,8 +375,8 @@ mod tests {
|
|||
use super::*;
|
||||
use crate::init_test_env_logger;
|
||||
|
||||
fn posstate_from_pos(position: geo_types::Point) -> crate::applogic::PositionState {
|
||||
crate::applogic::PositionState {
|
||||
fn posstate_from_pos(position: geo_types::Point) -> esp32_cits_core::PosVel {
|
||||
esp32_cits_core::PosVel {
|
||||
position,
|
||||
heading_deg: None,
|
||||
speed_mps: None,
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@ pub struct State {
|
|||
time: chrono::NaiveDateTime,
|
||||
|
||||
#[allow(unused)]
|
||||
pos: PositionState,
|
||||
pos: esp32_cits_core::PosVel,
|
||||
|
||||
#[cfg(feature = "spat")]
|
||||
last_prune: chrono::NaiveDateTime,
|
||||
|
|
@ -130,15 +130,7 @@ pub struct GnssFix {
|
|||
pub speed_mps: Option<f32>,
|
||||
}
|
||||
|
||||
#[allow(unused)]
|
||||
#[derive(Debug, Default, Clone, PartialEq)]
|
||||
pub struct PositionState {
|
||||
pub position: geo_types::Point,
|
||||
pub heading_deg: Option<f32>,
|
||||
pub speed_mps: Option<f32>,
|
||||
}
|
||||
|
||||
impl From<&GnssFix> for PositionState {
|
||||
impl From<&GnssFix> for esp32_cits_core::PosVel {
|
||||
fn from(value: &GnssFix) -> Self {
|
||||
let position = geo_types::Point::new(value.longitude_deg, value.latitude_deg);
|
||||
|
||||
|
|
@ -149,7 +141,7 @@ impl From<&GnssFix> for PositionState {
|
|||
}
|
||||
}
|
||||
}
|
||||
impl From<GnssFix> for PositionState {
|
||||
impl From<GnssFix> for esp32_cits_core::PosVel {
|
||||
fn from(value: GnssFix) -> Self {
|
||||
(&value).into()
|
||||
}
|
||||
|
|
@ -212,7 +204,7 @@ impl State {
|
|||
Self {
|
||||
initialized: false,
|
||||
time: chrono::NaiveDateTime::default(),
|
||||
pos: PositionState::default(),
|
||||
pos: esp32_cits_core::PosVel::default(),
|
||||
#[cfg(feature = "spat")]
|
||||
last_prune: chrono::NaiveDateTime::default(),
|
||||
#[cfg(feature = "spat")]
|
||||
|
|
@ -641,7 +633,7 @@ impl State {
|
|||
}
|
||||
|
||||
#[allow(unused)]
|
||||
pub fn pos(&self) -> &PositionState {
|
||||
pub fn pos(&self) -> &esp32_cits_core::PosVel {
|
||||
&self.pos
|
||||
}
|
||||
}
|
||||
|
|
|
|||
18
src/io.rs
18
src/io.rs
|
|
@ -169,7 +169,7 @@ impl From<msg::PositionState> for crate::applogic::GnssFix {
|
|||
}
|
||||
}
|
||||
|
||||
impl From<msg::PositionState> for crate::applogic::PositionState {
|
||||
impl From<msg::PositionState> for esp32_cits_core::PosVel {
|
||||
fn from(value: msg::PositionState) -> Self {
|
||||
let position = geo_types::Point::new(
|
||||
value.position.longitude_deg.into(),
|
||||
|
|
@ -194,7 +194,7 @@ impl From<msg::PositionState> for crate::applogic::PositionState {
|
|||
}
|
||||
}
|
||||
|
||||
impl From<msg::ItsPosition> for crate::applogic::PositionState {
|
||||
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.;
|
||||
let longitude_deg = f64::from(value.longitude) / 10_000_000.;
|
||||
|
|
@ -208,11 +208,11 @@ impl From<msg::ItsPosition> for crate::applogic::PositionState {
|
|||
}
|
||||
}
|
||||
|
||||
impl crate::v2x_tx::GnItsPayload for msg::RawMsgTx {
|
||||
impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx {
|
||||
fn make_eh(
|
||||
&self,
|
||||
address: [u8; 6],
|
||||
own_position: &crate::applogic::PositionState,
|
||||
own_position: &esp32_cits_core::PosVel,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
seq_no: &mut u16,
|
||||
) -> Result<
|
||||
|
|
@ -275,19 +275,19 @@ impl crate::v2x_tx::GnItsPayload for msg::RawMsgTx {
|
|||
*seq_no += 1;
|
||||
|
||||
(
|
||||
crate::v2x_tx::gn::make_tsb_eh(
|
||||
esp32_cits_core::tx::gn::make_tsb_eh(
|
||||
address,
|
||||
station_type,
|
||||
own_position,
|
||||
time,
|
||||
*seq_no,
|
||||
),
|
||||
)?,
|
||||
None,
|
||||
hop_limit,
|
||||
)
|
||||
}
|
||||
msg::GnTransport::Shb => (
|
||||
crate::v2x_tx::gn::make_shb_eh(address, station_type, own_position, time),
|
||||
esp32_cits_core::tx::gn::make_shb_eh(address, station_type, own_position, time)?,
|
||||
None,
|
||||
1,
|
||||
),
|
||||
|
|
@ -308,7 +308,7 @@ impl msg::RawMsgTx {
|
|||
fn make_geobcast(
|
||||
address: [u8; 6],
|
||||
station_type: geonetworking::en302636_4_1::StationType,
|
||||
own_position: &crate::applogic::PositionState,
|
||||
own_position: &esp32_cits_core::PosVel,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
gn_area: msg::GnArea,
|
||||
sequence_number: u16,
|
||||
|
|
@ -320,7 +320,7 @@ impl msg::RawMsgTx {
|
|||
alloc::string::String,
|
||||
> {
|
||||
let source_position_vector =
|
||||
crate::v2x_tx::gn::make_lpv(address, station_type, own_position, time);
|
||||
esp32_cits_core::tx::gn::make_lpv(address, station_type, own_position, time)?;
|
||||
|
||||
let (atype, dist_a, dist_b, angle) = match gn_area.area() {
|
||||
msg::GnAreaShape::EtsiAreashapeCircle => (
|
||||
|
|
|
|||
62
src/main.rs
62
src/main.rs
|
|
@ -38,7 +38,7 @@ use esp_hal::clock::CpuClock;
|
|||
use esp_hal::timer::timg::TimerGroup;
|
||||
#[cfg(all(target_arch = "riscv32", feature = "spat"))]
|
||||
use esp_println::println;
|
||||
use esp_radio::wifi;
|
||||
use esp32_cits_core::radio;
|
||||
use geonetworking::Decode as _;
|
||||
use log::{error, info, warn};
|
||||
|
||||
|
|
@ -49,13 +49,10 @@ mod cache;
|
|||
mod geo_alg;
|
||||
#[cfg(feature = "uart")]
|
||||
mod io;
|
||||
mod radio;
|
||||
#[cfg(feature = "screen")]
|
||||
mod screen;
|
||||
#[cfg(feature = "gnss_ubx")]
|
||||
mod ubx;
|
||||
#[cfg(any(feature = "cam_tx", feature = "uart"))]
|
||||
mod v2x_tx;
|
||||
|
||||
const WIFI_CHANNEL: radio::Channel = 180;
|
||||
const WIFI_TXPOWER: i8 = 10; // dBm, 2..20
|
||||
|
|
@ -244,7 +241,7 @@ async fn main(spawner: Spawner) -> ! {
|
|||
let mut state = applogic::State::new();
|
||||
#[cfg(feature = "cam_tx")]
|
||||
let mut cam = {
|
||||
let station_id = v2x_tx::make_station_id(mac);
|
||||
let station_id = make_station_id(mac);
|
||||
info!("V2X Station ID: {station_id}");
|
||||
|
||||
applogic::cam_tx::CamState::new(
|
||||
|
|
@ -289,7 +286,7 @@ async fn main(spawner: Spawner) -> ! {
|
|||
});
|
||||
}
|
||||
Some(io::msg::serial_input_msg::Payload::TxMsg(tx_request)) => {
|
||||
use v2x_tx::GnItsPayload as _;
|
||||
use esp32_cits_core::tx::GnItsPayload as _;
|
||||
|
||||
info!(
|
||||
"UART: Got new TX message: Type {} with {} bytes",
|
||||
|
|
@ -380,7 +377,7 @@ async fn main(spawner: Spawner) -> ! {
|
|||
|
||||
#[cfg(feature = "cam_tx")]
|
||||
{
|
||||
use v2x_tx::GnItsPayload;
|
||||
use esp32_cits_core::tx::GnItsPayload as _;
|
||||
let time = state.time().and_utc();
|
||||
|
||||
cam.update(time, fix.clone().into());
|
||||
|
|
@ -513,7 +510,7 @@ async fn main(spawner: Spawner) -> ! {
|
|||
reason = "adhering to callback interface"
|
||||
)]
|
||||
#[allow(clippy::too_many_lines)]
|
||||
fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
|
||||
fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) {
|
||||
#[cfg(feature = "spat")]
|
||||
const PRUNE_INTERVAL: chrono::Duration = chrono::Duration::seconds(60);
|
||||
#[cfg(feature = "spat")]
|
||||
|
|
@ -534,27 +531,18 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
|
|||
.naive_utc()
|
||||
};
|
||||
|
||||
// Ignore frames with errors and non-data frames
|
||||
if frame.rx_cntl.rx_state != 0 {
|
||||
warn!("Received frame has RX error: {}", frame.rx_cntl.rx_state);
|
||||
return;
|
||||
}
|
||||
if !radio::PromiscuousPktType::Data.matches(frame.frame_type) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Ignore non-broadcast frames
|
||||
if frame.len < (4 + 6) {
|
||||
warn!(
|
||||
"Received frame too small to be a valid broadcast frame: {} bytes",
|
||||
frame.len
|
||||
);
|
||||
return;
|
||||
}
|
||||
let dest_mac = &frame.data[4..10];
|
||||
if dest_mac != [0xff; 6] {
|
||||
warn!("Received frame is not broadcast frame: Dest MAC {dest_mac:02x?}");
|
||||
return;
|
||||
match esp32_cits_core::rx::is_broadcast_frame(&frame) {
|
||||
Err(err) => {
|
||||
warn!("{err}");
|
||||
return;
|
||||
}
|
||||
Ok(false) => {
|
||||
// drop frame
|
||||
return;
|
||||
}
|
||||
Ok(true) => {
|
||||
// continue
|
||||
}
|
||||
}
|
||||
|
||||
// info!("Received frame with {} bytes", frame.len);
|
||||
|
|
@ -885,3 +873,19 @@ fn send_uart0(data: &[u8]) {
|
|||
}
|
||||
});
|
||||
}
|
||||
|
||||
#[cfg(all(feature = "esp", feature = "cam_tx"))]
|
||||
/// Creates a station ID from the WLAN MAC address
|
||||
///
|
||||
/// Will use `0xC173` as static prefix and uses last 16 bit from MAC address for the remainder.
|
||||
///
|
||||
/// # Panics
|
||||
/// Won't panic in practive b/c of pre-conditions
|
||||
#[must_use]
|
||||
pub fn make_station_id(mac: esp_hal::efuse::MacAddress) -> u32 {
|
||||
// unwrap is fine here since we can be sure that the MacAddress is 6 byte
|
||||
// and a slice of 2 bytes can be converted to [u8; 2]
|
||||
let mac_last_16bit = u16::from_be_bytes(mac.as_bytes().get(4..6).unwrap().try_into().unwrap());
|
||||
|
||||
0xC173_0000 | u32::from(mac_last_16bit)
|
||||
}
|
||||
|
|
|
|||
288
src/radio.rs
288
src/radio.rs
|
|
@ -1,288 +0,0 @@
|
|||
//! IEEE 802.11p C-ITS Capture with ESP32-C5
|
||||
#![allow(unused)]
|
||||
|
||||
// -----------------------------
|
||||
// missing interface definitions
|
||||
// -----------------------------
|
||||
|
||||
pub type Channel = core::ffi::c_int;
|
||||
|
||||
unsafe extern "C" {
|
||||
/// Changes the 802.11 PHY channel
|
||||
///
|
||||
/// C function signature: `void phy_change_channel(int,int,int,int)`
|
||||
///
|
||||
/// Options:
|
||||
/// - `primary`: Channel frequency in MHz
|
||||
/// - `ignored1`: Ignored (set to 1)
|
||||
/// - `ignored2`: Ignored (set to 0)
|
||||
/// - `ht_mode`: Probably HT mode
|
||||
unsafe fn phy_change_channel(
|
||||
primary: Channel,
|
||||
ignored1: core::ffi::c_int,
|
||||
ignored2: core::ffi::c_int,
|
||||
ht_mode: core::ffi::c_int,
|
||||
) -> core::ffi::c_int;
|
||||
|
||||
/// Enables 802.11p mode on the ESP32-C5
|
||||
///
|
||||
/// C function signature: `void phy_11p_set(int,int)`
|
||||
///
|
||||
/// Options:
|
||||
/// - `enable`: Boolean option to enable 802.11p mode
|
||||
/// - `half_rate`: 5 MHz mode
|
||||
unsafe fn phy_11p_set(enable: u8, half_rate: u8);
|
||||
|
||||
}
|
||||
|
||||
/// Enum values for `wifi_promiscuous_pkt_type_t` used in `esp_radio::wifi::sniffer::PromiscuousPkt::frame_type`
|
||||
///
|
||||
/// Original value is actually `core::ffi::c_uint`, but that can't be used in `#[repr()]` attributes,
|
||||
/// so we're using `u8` as the smallest suitable type here that can be casted to a bigger type.
|
||||
#[repr(u8)]
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
#[allow(dead_code)]
|
||||
pub(crate) enum PromiscuousPktType {
|
||||
/// Management frame, indicates 'buf' argument is `wifi_promiscuous_pkt_t`
|
||||
Mgmt = 0,
|
||||
/// Control frame, indicates 'buf' argument is `wifi_promiscuous_pkt_t`
|
||||
Ctrl = 1,
|
||||
/// Data frame, indicates 'buf' argument is `wifi_promiscuous_pkt_t`
|
||||
Data = 2,
|
||||
/// Other type, such as MIMO etc. 'buf' argument is `wifi_promiscuous_pkt_t` but the payload is zero length.
|
||||
Misc = 3,
|
||||
}
|
||||
|
||||
impl PromiscuousPktType {
|
||||
#[allow(dead_code)]
|
||||
pub fn as_repr(self) -> u8 {
|
||||
self as u8
|
||||
}
|
||||
|
||||
/// Converts to underlying type of `wifi_promiscuous_pkt_type_t` (used by `esp_radio::wifi::sniffer::PromiscuousPkt::frame_type`)
|
||||
pub fn as_frame_type(self) -> core::ffi::c_uint {
|
||||
core::ffi::c_uint::from(self.as_repr())
|
||||
}
|
||||
|
||||
/// Determines if we are the same frame type as `esp_radio::wifi::sniffer::PromiscuousPkt::frame_type`
|
||||
pub fn matches(self, frame_type: core::ffi::c_uint) -> bool {
|
||||
self.as_frame_type() == frame_type
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "esp")]
|
||||
/// Configure WiFi Sniffer to a certain 802.11p channel
|
||||
pub fn setup_wifi_sniffer(
|
||||
channel: Channel,
|
||||
sniffer: &mut esp_radio::wifi::sniffer::Sniffer,
|
||||
callback: fn(esp_radio::wifi::sniffer::PromiscuousPkt<'_>),
|
||||
) -> Result<(), alloc::string::String> {
|
||||
sniffer
|
||||
.set_promiscuous_mode(true)
|
||||
.map_err(|err| alloc::format!("Failed to enable promiscuous mode: {err:?}"))?;
|
||||
|
||||
sniffer.set_receive_cb(callback);
|
||||
|
||||
// convert channel number to frequency in MHz
|
||||
let primary = 5000 + (channel * 5);
|
||||
|
||||
unsafe {
|
||||
phy_11p_set(1, 0);
|
||||
match phy_change_channel(primary, 1, 0, 0) {
|
||||
0 => Ok(()),
|
||||
ret => Err(alloc::format!("Failed to change channel: {ret}")),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "esp")]
|
||||
/// Sends 802.11p broadcast frame
|
||||
///
|
||||
/// Expects `data` to contain GN header (BTP header and ITS payload) to be a valid C-ITS/ ITS-G5 packet/ frame.
|
||||
///
|
||||
/// # Errors
|
||||
/// Fails when MAC is not the right size or returns frame send error
|
||||
pub fn send_80211_bcast_frame(
|
||||
sniffer: &mut esp_radio::wifi::sniffer::Sniffer,
|
||||
mac_addr: &[u8],
|
||||
data: &[u8],
|
||||
) -> Result<(), alloc::string::String> {
|
||||
const HEADER_SIZE: usize = 24 + 8;
|
||||
|
||||
let mac = mac_addr
|
||||
.try_into()
|
||||
.map_err(|err| alloc::format!("Unexpected MAC address size: {err}"))?;
|
||||
|
||||
// assemble frame
|
||||
let mut frame_buf = alloc::vec::Vec::with_capacity(HEADER_SIZE + data.len());
|
||||
frame_buf.extend_from_slice(&DataFrameHeader::new_80211p(mac).to_binary());
|
||||
frame_buf.extend_from_slice(&LlcHeader::new_80211p().to_binary());
|
||||
frame_buf.extend_from_slice(data);
|
||||
|
||||
// send
|
||||
sniffer
|
||||
.send_raw_frame(true, &frame_buf, true)
|
||||
.map_err(|err| alloc::format!("Failed to send 802.11p frame: {err:?}"))
|
||||
}
|
||||
|
||||
/// IEEE 802.11p frame header
|
||||
#[derive(Debug, Default)]
|
||||
struct DataFrameHeader {
|
||||
/// byte 0: subtype in upper nibble, type in lower nibble
|
||||
/// "normal" data frame: 0x08, QoS data frame: 0x88
|
||||
pub type_and_subtype: u8,
|
||||
/// byte 1: frame contol field, 0x00 for 802.11p
|
||||
pub fcf_flags: u8,
|
||||
/// byte 2..3: duration in micros, usually 0x0000 for 802.11p (toDS = 0, fromDS = 0)
|
||||
pub duration: u16,
|
||||
/// byte 4..9: receiver/ destination for 802.11p
|
||||
pub address_1: [u8; 6],
|
||||
/// byte 10..15: transmitter/ source for 802.11p
|
||||
pub address_2: [u8; 6],
|
||||
/// byte 16..21: BSSID for 802.11p
|
||||
pub address_3: [u8; 6],
|
||||
/// byte 22..24: upper 12 bits: seq. no, lower 4 bits: fragment no
|
||||
pub sequence_control: u16,
|
||||
// Note: address 4 is not used in 802.11p
|
||||
// pub qos: Option<[u8; 2]>, // QoS frames are supported by radio library
|
||||
// Note: 802.11p does not use HT mode(s)
|
||||
}
|
||||
|
||||
impl DataFrameHeader {
|
||||
const BCAST_MAC: [u8; 6] = [0xFF; 6];
|
||||
|
||||
/// Create an 802.11p (non-QoS) data frame header with a source MAC address
|
||||
pub(crate) fn new_80211p(source_mac: &[u8; 6]) -> Self {
|
||||
Self {
|
||||
type_and_subtype: 0x08,
|
||||
address_1: Self::BCAST_MAC,
|
||||
address_2: *source_mac,
|
||||
address_3: Self::BCAST_MAC,
|
||||
..Default::default()
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn to_binary(&self) -> [u8; 24] {
|
||||
let mut buf = [0u8; 24];
|
||||
|
||||
buf[0] = self.type_and_subtype;
|
||||
|
||||
buf[1] = self.fcf_flags;
|
||||
|
||||
Self::write_u16(&mut buf, 2, self.duration);
|
||||
Self::write_mac_addr(&mut buf, 4, &self.address_1);
|
||||
Self::write_mac_addr(&mut buf, 10, &self.address_2);
|
||||
Self::write_mac_addr(&mut buf, 16, &self.address_3);
|
||||
Self::write_u16(&mut buf, 22, self.sequence_control);
|
||||
|
||||
buf
|
||||
}
|
||||
|
||||
fn write_u16(buf: &mut [u8; 24], offset: usize, data: u16) {
|
||||
for (idx, byte) in data.to_be_bytes().iter().enumerate() {
|
||||
buf[idx + offset] = *byte;
|
||||
}
|
||||
}
|
||||
|
||||
fn write_mac_addr(buf: &mut [u8; 24], offset: usize, data: &[u8; 6]) {
|
||||
for (idx, byte) in data.iter().enumerate() {
|
||||
buf[idx + offset] = *byte;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// IEEE 802.11p LLC header
|
||||
#[derive(Debug, Default)]
|
||||
struct LlcHeader {
|
||||
/// byte 0: SNAP (0xaa) in 802.11p
|
||||
pub dsap: u8, // SNAP in
|
||||
/// byte 1: SNAP (0xaa) in 802.11p
|
||||
pub lsap: u8, // SNAP in
|
||||
/// byte 2: SNAP (0xaa) in 802.11p
|
||||
pub control_1: u8, // 0x03
|
||||
// Note: optional second control byte not used in 802.11p
|
||||
|
||||
// SNAP fields
|
||||
/// set to 0 since ethertype is used for protocol ID in 802.11p
|
||||
pub oui: [u8; 3],
|
||||
/// ethertype GeoNetworking (0x8947) in 802.11p
|
||||
pub protocol_id: u16,
|
||||
}
|
||||
|
||||
impl LlcHeader {
|
||||
pub(crate) fn new_80211p() -> Self {
|
||||
Self {
|
||||
dsap: 0xaa,
|
||||
lsap: 0xaa,
|
||||
control_1: 0x03,
|
||||
oui: [0; 3],
|
||||
protocol_id: 0x8947,
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn to_binary(&self) -> [u8; 8] {
|
||||
let mut buf: [u8; 8] = [
|
||||
self.dsap,
|
||||
self.lsap,
|
||||
self.control_1,
|
||||
0x00,
|
||||
0x00,
|
||||
0x00,
|
||||
0x00,
|
||||
0x00,
|
||||
];
|
||||
|
||||
buf[3] = self.oui[0];
|
||||
buf[4] = self.oui[1];
|
||||
buf[5] = self.oui[2];
|
||||
|
||||
buf[6] = self.protocol_id.to_be_bytes()[0];
|
||||
buf[7] = self.protocol_id.to_be_bytes()[1];
|
||||
|
||||
buf
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn data_frame_test() {
|
||||
// build a typical IEEE 802.11p frame
|
||||
const OWN_MAC: [u8; 6] = [0x12, 0x34, 0x56, 0x78, 0xab, 0xcd];
|
||||
const REF_HEADER: [u8; 24] = [
|
||||
0x08, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x12, 0x34, 0x56, 0x78,
|
||||
0xab, 0xcd, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00,
|
||||
];
|
||||
|
||||
let frame = DataFrameHeader::new_80211p(&OWN_MAC);
|
||||
|
||||
assert_eq!(0x08, frame.type_and_subtype); // data frame, subtype 0
|
||||
assert_eq!(0x0000, frame.fcf_flags);
|
||||
assert_eq!(0x0000, frame.duration);
|
||||
assert_eq!([0xFF; 6], frame.address_1);
|
||||
assert_eq!(OWN_MAC, frame.address_2);
|
||||
assert_eq!([0xFF; 6], frame.address_3);
|
||||
assert_eq!(0x0000, frame.sequence_control);
|
||||
|
||||
assert_eq!(REF_HEADER, frame.to_binary());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn llc_test() {
|
||||
// build a typical IEEE 802.11p LLC header
|
||||
const REF_HEADER: [u8; 8] = [0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00, 0x89, 0x47];
|
||||
|
||||
let llc = LlcHeader::new_80211p();
|
||||
|
||||
assert_eq!(0xaa, llc.dsap);
|
||||
assert_eq!(0xaa, llc.lsap);
|
||||
assert_eq!(0x03, llc.control_1);
|
||||
assert_eq!([0x00; 3], llc.oui);
|
||||
assert_eq!(0x8947, llc.protocol_id);
|
||||
|
||||
assert_eq!(REF_HEADER, llc.to_binary());
|
||||
}
|
||||
}
|
||||
|
|
@ -7,9 +7,7 @@ mod applogic;
|
|||
mod cache;
|
||||
mod geo_alg;
|
||||
mod io;
|
||||
mod radio;
|
||||
mod testdata;
|
||||
mod v2x_tx;
|
||||
|
||||
fn main() {
|
||||
// env_logger::init();
|
||||
|
|
|
|||
|
|
@ -90,9 +90,9 @@ impl<'a> TryFrom<&ublox::nav_pvt::proto27::NavPvtRef<'a>> for crate::applogic::G
|
|||
|
||||
let latitude_deg = pos.lat;
|
||||
let longitude_deg = pos.lon;
|
||||
#[allow(clippy::cast_precision_loss)]
|
||||
#[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
|
||||
let heading_deg = vel.heading as f32;
|
||||
#[allow(clippy::cast_precision_loss)]
|
||||
#[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
|
||||
let speed_mps = vel.speed as f32;
|
||||
|
||||
Ok(Self {
|
||||
|
|
|
|||
|
|
@ -1,94 +0,0 @@
|
|||
//! Geonetworking utilities
|
||||
|
||||
use c_its_parser::gn as geonetworking;
|
||||
|
||||
#[allow(unused)]
|
||||
pub fn make_shb_eh(
|
||||
address: [u8; 6],
|
||||
station_type: geonetworking::en302636_4_1::StationType,
|
||||
own_position: &crate::applogic::PositionState,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
) -> geonetworking::en302636_4_1::ExtendedHeader {
|
||||
let source_position_vector = make_lpv(address, station_type, own_position, time);
|
||||
|
||||
geonetworking::en302636_4_1::ExtendedHeader::SHB(
|
||||
geonetworking::en302636_4_1::SingleHopBroadcast {
|
||||
source_position_vector,
|
||||
media_dependent_data: [0; 4],
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
#[allow(unused)]
|
||||
pub fn make_tsb_eh(
|
||||
address: [u8; 6],
|
||||
station_type: geonetworking::en302636_4_1::StationType,
|
||||
own_position: &crate::applogic::PositionState,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
sequence_number: u16,
|
||||
) -> geonetworking::en302636_4_1::ExtendedHeader {
|
||||
let source_position_vector = make_lpv(address, station_type, own_position, time);
|
||||
|
||||
geonetworking::en302636_4_1::ExtendedHeader::TSB(
|
||||
geonetworking::en302636_4_1::TopologicallyScopedBroadcast::new(
|
||||
sequence_number,
|
||||
source_position_vector,
|
||||
),
|
||||
)
|
||||
}
|
||||
|
||||
#[allow(unused)]
|
||||
pub fn make_gbc_eh(
|
||||
address: [u8; 6],
|
||||
station_type: geonetworking::en302636_4_1::StationType,
|
||||
own_position: &crate::applogic::PositionState,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
target_pos: geo_types::Point,
|
||||
radius_m: u16,
|
||||
sequence_number: u16,
|
||||
) -> geonetworking::en302636_4_1::ExtendedHeader {
|
||||
let source_position_vector = make_lpv(address, station_type, own_position, time);
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
let latitude_deg = target_pos.x() as f32;
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
let longitude_deg = target_pos.y() as f32;
|
||||
|
||||
geonetworking::en302636_4_1::ExtendedHeader::GBC(
|
||||
geonetworking::en302636_4_1::GeoBroadcast::try_from_values(
|
||||
sequence_number,
|
||||
source_position_vector,
|
||||
latitude_deg,
|
||||
longitude_deg,
|
||||
radius_m,
|
||||
0,
|
||||
0,
|
||||
)
|
||||
.expect("Failed to create circular GBC"),
|
||||
)
|
||||
}
|
||||
|
||||
pub fn make_lpv(
|
||||
address: [u8; 6],
|
||||
station_type: geonetworking::en302636_4_1::StationType,
|
||||
own_position: &crate::applogic::PositionState,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
) -> geonetworking::en302636_4_1::LongPositionVector {
|
||||
let timestamp_its = c_its_parser::time_utils::TimestampIts::from(time);
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
let latitude_deg = own_position.position.y() as f32;
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
let longitude_deg = own_position.position.x() as f32;
|
||||
let speed_mps = own_position.speed_mps.unwrap_or_default();
|
||||
let heading_deg = own_position.heading_deg.unwrap_or_default();
|
||||
|
||||
geonetworking::en302636_4_1::LongPositionVector::try_from_values(
|
||||
geonetworking::en302636_4_1::Address::new(false, station_type, address),
|
||||
timestamp_its.0,
|
||||
latitude_deg,
|
||||
longitude_deg,
|
||||
true,
|
||||
speed_mps,
|
||||
heading_deg,
|
||||
)
|
||||
.expect("Failed to create LongPositionVector")
|
||||
}
|
||||
|
|
@ -1,164 +0,0 @@
|
|||
//! V2X Transmission
|
||||
|
||||
use alloc::vec::Vec;
|
||||
|
||||
use c_its_parser::gn::{self as geonetworking, Encode};
|
||||
|
||||
pub mod gn;
|
||||
|
||||
#[cfg(all(feature = "esp", feature = "cam_tx"))]
|
||||
/// Creates a station ID from the WLAN MAC address
|
||||
///
|
||||
/// Will use `0xC173` as static prefix and uses last 16 bit from MAC address for the remainder.
|
||||
pub fn make_station_id(mac: esp_hal::efuse::MacAddress) -> u32 {
|
||||
// unwrap is fine here since we can be sure that the MacAddress is 6 byte
|
||||
// and a slice of 2 bytes can be converted to [u8; 2]
|
||||
let mac_last_16bit = u16::from_be_bytes(mac.as_bytes().get(4..6).unwrap().try_into().unwrap());
|
||||
|
||||
0xC173_0000 | u32::from(mac_last_16bit)
|
||||
}
|
||||
|
||||
pub trait GnItsPayload {
|
||||
// common GN parameters
|
||||
const LIFETIME_MILLIS: u32 = 60_000; // itsGnDefaultPacketLifetime is 60 seconds according to ETSI EN 302 636-4-1 V1.4.1
|
||||
|
||||
// ---------------------------------------------
|
||||
// methods to build a GN packet with ITS payload
|
||||
// ---------------------------------------------
|
||||
|
||||
/// Builds the [`geonetworking::en302636_4_1::ExtendedHeader`] and additional information for the ITS message
|
||||
///
|
||||
/// Output:
|
||||
/// - Extended header
|
||||
/// - Area type (when using GAC or GBC)
|
||||
/// - hop limit
|
||||
///
|
||||
/// Note: Make sure to increase the sequence number on multi-hop packets.
|
||||
fn make_eh(
|
||||
&self,
|
||||
address: [u8; 6],
|
||||
own_position: &crate::applogic::PositionState,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
seq_no: &mut u16,
|
||||
) -> Result<
|
||||
(
|
||||
geonetworking::en302636_4_1::ExtendedHeader,
|
||||
Option<geonetworking::en302636_4_1::AreaType>,
|
||||
u8,
|
||||
),
|
||||
alloc::string::String,
|
||||
>;
|
||||
|
||||
/// Creates the ITS payload (without BTP header)
|
||||
fn make_payload(
|
||||
&self,
|
||||
) -> Result<(Vec<u8>, c_its_parser::standards::extensions::ItsMessageId), alloc::string::String>;
|
||||
|
||||
// ---------------------------------------------
|
||||
// common methods (with implementation)
|
||||
// ---------------------------------------------
|
||||
|
||||
/// Encodes the GN packet
|
||||
fn encode_packet(
|
||||
&self,
|
||||
address: [u8; 6],
|
||||
own_position: &crate::applogic::PositionState,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
is_mobile: bool,
|
||||
seq_no: &mut u16,
|
||||
) -> Result<Vec<u8>, alloc::string::String> {
|
||||
// Build payload (including BTP header)
|
||||
let (mut its_payload, msg_type) = self.make_payload()?;
|
||||
let mut payload = make_btp_b(msg_type).encode()?;
|
||||
|
||||
payload.append(&mut its_payload);
|
||||
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
let payload_length = (payload.len()) as u16;
|
||||
|
||||
// get extended header (depending on message type and sometimes content as well)
|
||||
let (eh, area_type, maximum_hop_limit) =
|
||||
self.make_eh(address, own_position, time, seq_no)?;
|
||||
|
||||
// Build GN headers
|
||||
let traffic_class = geonetworking::en302636_4_1::TrafficClass::try_new(
|
||||
false,
|
||||
false,
|
||||
make_traffic_class(msg_type),
|
||||
)
|
||||
.map_err(|err| alloc::format!("Failed to create TrafficClass: {err:?}"))?;
|
||||
let common = geonetworking::en302636_4_1::CommonHeader::from_values(
|
||||
geonetworking::en302636_4_1::NextAfterCommon::BTPB,
|
||||
geonetworking::en302636_4_1::HeaderType::new_from(&eh, area_type)?,
|
||||
traffic_class,
|
||||
is_mobile,
|
||||
payload_length,
|
||||
maximum_hop_limit,
|
||||
);
|
||||
|
||||
let remaining_hop_limit = maximum_hop_limit;
|
||||
let basic = geonetworking::en302636_4_1::BasicHeader::try_new(
|
||||
1,
|
||||
geonetworking::en302636_4_1::NextAfterBasic::CommonHeader,
|
||||
geonetworking::en302636_4_1::Lifetime::from_milliseconds(Self::LIFETIME_MILLIS),
|
||||
remaining_hop_limit,
|
||||
)
|
||||
.map_err(|err| alloc::format!("Failed to create BasicHeader: {err}"))?;
|
||||
|
||||
// Assemble and encode packet
|
||||
let packet = geonetworking::Packet::Unsecured {
|
||||
basic,
|
||||
common,
|
||||
extended: Some(eh),
|
||||
payload: &payload,
|
||||
};
|
||||
|
||||
packet
|
||||
.encode_to_vec()
|
||||
.map_err(|err| alloc::format!("Failed to encode GN packet: {err:?}"))
|
||||
}
|
||||
}
|
||||
|
||||
/// Creates a BTP-B header based on the ITS message ID (message type)
|
||||
///
|
||||
/// Port numbers and destination port info according to ETSI TS 103 248
|
||||
fn make_btp_b(
|
||||
msg_type: c_its_parser::standards::extensions::ItsMessageId,
|
||||
) -> c_its_parser::transport::TransportHeader {
|
||||
let (destination_port, destination_port_info) = match msg_type {
|
||||
c_its_parser::standards::extensions::ItsMessageId::Denm => (2002, 0),
|
||||
c_its_parser::standards::extensions::ItsMessageId::Cam => (2001, 0),
|
||||
c_its_parser::standards::extensions::ItsMessageId::Mapem => (2003, 0),
|
||||
c_its_parser::standards::extensions::ItsMessageId::Spatem => (2004, 0),
|
||||
c_its_parser::standards::extensions::ItsMessageId::Saem => (2005, 0),
|
||||
c_its_parser::standards::extensions::ItsMessageId::Ivim => (2006, 0),
|
||||
c_its_parser::standards::extensions::ItsMessageId::Srem => (2007, 0),
|
||||
c_its_parser::standards::extensions::ItsMessageId::Ssem => (2008, 0),
|
||||
c_its_parser::standards::extensions::ItsMessageId::Cpm => (2009, 0),
|
||||
c_its_parser::standards::extensions::ItsMessageId::Poi => (2010, 0),
|
||||
c_its_parser::standards::extensions::ItsMessageId::EvRsr => (2012, 0),
|
||||
c_its_parser::standards::extensions::ItsMessageId::Rtcmem => (2013, 0),
|
||||
_ => (0, 0), // no information found for the subsequent types
|
||||
};
|
||||
|
||||
c_its_parser::transport::TransportHeader::BtpB(c_its_parser::transport::BasicTransportBHeader {
|
||||
destination_port,
|
||||
destination_port_info,
|
||||
})
|
||||
}
|
||||
|
||||
/// Creates a traffic class ID based on the ITS message ID (message type)
|
||||
fn make_traffic_class(msg_type: c_its_parser::standards::extensions::ItsMessageId) -> u8 {
|
||||
#[allow(clippy::match_same_arms)]
|
||||
match msg_type {
|
||||
c_its_parser::standards::extensions::ItsMessageId::Denm => 1,
|
||||
c_its_parser::standards::extensions::ItsMessageId::Cam => 2,
|
||||
c_its_parser::standards::extensions::ItsMessageId::Mapem => 3, // from C-Roads RS_RSP_063(1)
|
||||
c_its_parser::standards::extensions::ItsMessageId::Spatem => 3, // from C-Roads RS_RSP_063(1)
|
||||
c_its_parser::standards::extensions::ItsMessageId::Ivim => 3, // from C-Roads RS_RSP_063(1)
|
||||
c_its_parser::standards::extensions::ItsMessageId::Srem => 2, // from C-Roads RS_RSP_063(1)
|
||||
c_its_parser::standards::extensions::ItsMessageId::Ssem => 2, // from C-Roads RS_RSP_063(1)
|
||||
c_its_parser::standards::extensions::ItsMessageId::Rtcmem => 3, // CSP_MaxPrio=252 -> 3
|
||||
_ => 3, // fall-back to lowest priority
|
||||
}
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue