Implement UART I/O protocol
This commit is contained in:
Jannik Beyerstedt
parent
ba71e718df
commit
4d7bef58a1
11 changed files with 610 additions and 36 deletions
10
Cargo.toml
10
Cargo.toml
|
|
@ -34,11 +34,11 @@ esp = [
|
|||
std = ["c-its-parser/std"]
|
||||
|
||||
# enable L67K GNSS module
|
||||
gnss = ["dep:embassy_gps"]
|
||||
gnss = ["_gnss", "dep:embassy_gps"]
|
||||
# echo CAM data on serial
|
||||
cam = ["c-its-parser/cam"]
|
||||
# send CAM autonomously
|
||||
cam_tx = ["c-its-parser/cam", "gnss"]
|
||||
cam_tx = ["c-its-parser/cam"]
|
||||
# echo DENM events on serial and BLE
|
||||
denm = ["c-its-parser/denm"]
|
||||
# evaluate SPAT information
|
||||
|
|
@ -47,9 +47,13 @@ spat = ["c-its-parser/spatem", "c-its-parser/mapem"]
|
|||
spat_debug = []
|
||||
# enable ST7789 based display
|
||||
screen = ["dep:mipidsi", "dep:embedded-graphics", "dep:embedded-hal-bus", "dep:profont"]
|
||||
# enable I/O via UART
|
||||
uart = ["_uart", "_gnss", "dep:prost"]
|
||||
|
||||
# internal feature, that any UART I/O is enabled
|
||||
_uart = []
|
||||
# internal feature, that any GNSS source is enabled
|
||||
_gnss = []
|
||||
|
||||
[dependencies]
|
||||
esp-hal = { version = "~1.1.0", optional = true, features = ["esp32c5", "log-04", "unstable"] }
|
||||
|
|
@ -102,7 +106,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"] }
|
||||
|
||||
[dev-dependencies]
|
||||
assert_float_eq = "1.2.0"
|
||||
|
|
|
|||
|
|
@ -32,6 +32,9 @@ Just prints a line for each received CAM.
|
|||
Generates and transmits a CAM when a GNSS update was received.
|
||||
Depends on the `gnss` feature.
|
||||
|
||||
### UART
|
||||
Enable UART I/O protocol according to [docs/uart-protocol.md](./docs/uart-protocol.md).
|
||||
|
||||
|
||||
## Usage
|
||||
|
||||
|
|
@ -64,13 +67,13 @@ Additional features are available through feature flags:
|
|||
- `spat_debug`: Enable additional debug information for the SPAT/ GLOSA use case
|
||||
- `denm`: Enable DENMs
|
||||
- `cam`: Enable CAMs
|
||||
- `cam_tx`: Enable CAM generation and transmission
|
||||
- `cam_tx`: Enable CAM generation and transmission (needs time and position source)
|
||||
|
||||
Officially supported feature combinations are:
|
||||
- `esp,spat,gnss,screen` (default features)
|
||||
- `esp,spat,gnss,screen,cam_tx`
|
||||
- `esp,spat,gnss,screen,cam_tx,cam,denm`
|
||||
- To be implemented: No GNSS and screen when BLE/ UART interface is implemented
|
||||
- `esp,uart,cam,denm`
|
||||
|
||||
Connect your ESP32-C5 and run:
|
||||
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@ extended=${2:-false}
|
|||
|
||||
|
||||
features_base=(
|
||||
#cam,denm,uart
|
||||
uart,cam,denm
|
||||
spat,gnss,screen
|
||||
spat,gnss,screen,cam_tx
|
||||
)
|
||||
|
|
@ -17,7 +17,7 @@ features_more=(
|
|||
spat,gnss,screen,cam_tx,cam,denm
|
||||
)
|
||||
features_test=(
|
||||
spat,cam,denm
|
||||
spat,cam,denm,uart
|
||||
)
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -300,20 +300,23 @@ impl crate::v2x_tx::GnItsPayload for CamState {
|
|||
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,
|
||||
> {
|
||||
let station_type = geonetworking::en302636_4_1::StationType::try_from(self.station_type.0)
|
||||
.unwrap_or_default();
|
||||
|
||||
// CAM always uses SHB, hop-limit 1
|
||||
(
|
||||
Ok((
|
||||
crate::v2x_tx::gn::make_shb_eh(address, station_type, own_position, time),
|
||||
None,
|
||||
1,
|
||||
)
|
||||
))
|
||||
}
|
||||
|
||||
fn make_payload(
|
||||
|
|
|
|||
|
|
@ -639,6 +639,11 @@ impl State {
|
|||
pub fn time(&self) -> chrono::prelude::NaiveDateTime {
|
||||
self.time
|
||||
}
|
||||
|
||||
#[allow(unused)]
|
||||
pub fn pos(&self) -> &PositionState {
|
||||
&self.pos
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
|
|
|
|||
464
src/io.rs
464
src/io.rs
|
|
@ -5,6 +5,12 @@ pub mod msg {
|
|||
include!(concat!(env!("OUT_DIR"), "/c_its_io.rs"));
|
||||
}
|
||||
|
||||
use alloc::string::ToString;
|
||||
use alloc::vec::Vec;
|
||||
|
||||
use c_its_parser::gn as geonetworking;
|
||||
use log::error;
|
||||
|
||||
impl From<msg::RawMsgRx> for msg::SerialOutputMsg {
|
||||
fn from(value: msg::RawMsgRx) -> Self {
|
||||
use msg::serial_output_msg::Payload;
|
||||
|
|
@ -25,6 +31,144 @@ impl From<msg::DenmEvent> for msg::SerialOutputMsg {
|
|||
}
|
||||
}
|
||||
|
||||
impl msg::RawMsgRx {
|
||||
pub fn serialize_uart_proto(&self) -> Vec<u8> {
|
||||
serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(self.clone()))
|
||||
}
|
||||
}
|
||||
impl msg::DenmEvent {
|
||||
#[allow(unused)]
|
||||
pub fn serialize_uart_proto(&self) -> Vec<u8> {
|
||||
serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(*self))
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Default)]
|
||||
pub struct Parser {
|
||||
buffer: alloc::vec::Vec<u8>,
|
||||
}
|
||||
|
||||
impl Parser {
|
||||
const SOF_LEN: u16 = 4;
|
||||
const SOF1: u8 = 0x56;
|
||||
const SOF2: u8 = 0x32;
|
||||
const SOF3: u8 = 0x58;
|
||||
const SOF4: u8 = 0x2B;
|
||||
const EOF_LEN: u16 = 2;
|
||||
#[allow(unused)]
|
||||
const EOF1: u8 = 0x0d;
|
||||
#[allow(unused)]
|
||||
const EOF2: u8 = 0x0a;
|
||||
const LENGTH_LEN: u16 = 2;
|
||||
|
||||
/// Parses incoming data and return list of messages
|
||||
pub fn parse(&mut self, input: &[u8]) -> alloc::vec::Vec<msg::SerialInputMsg> {
|
||||
use prost::Message as _;
|
||||
|
||||
// combine with existing data
|
||||
self.buffer.append(&mut input.to_vec());
|
||||
|
||||
// find start of frame and parse
|
||||
let mut output = alloc::vec![];
|
||||
while let Some(pos) = Self::find_sof(&self.buffer) {
|
||||
// drop everything before the SOF
|
||||
self.buffer.drain(..pos);
|
||||
|
||||
// get length field and check buffer size
|
||||
if let Some(len_bytes) = self
|
||||
.buffer
|
||||
.get(usize::from(Self::SOF_LEN)..usize::from(Self::SOF_LEN + Self::LENGTH_LEN))
|
||||
{
|
||||
// unwrap is fine since we queried 2 bytes before, so we can also convert to 2-byte slice
|
||||
let msg_size = u16::from_be_bytes(len_bytes.try_into().unwrap());
|
||||
|
||||
let required_buf_size = Self::SOF_LEN + Self::LENGTH_LEN + msg_size + Self::EOF_LEN;
|
||||
if let Some(payload) = self.buffer.get(
|
||||
usize::from(Self::SOF_LEN + Self::LENGTH_LEN)
|
||||
..usize::from(required_buf_size - 2),
|
||||
) {
|
||||
match msg::SerialInputMsg::decode(payload) {
|
||||
Ok(data) => {
|
||||
output.push(data);
|
||||
}
|
||||
Err(err) => error!("Failed to parse input: {err:?}"),
|
||||
}
|
||||
|
||||
// consume buffer
|
||||
self.buffer.drain(..usize::from(required_buf_size));
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
output
|
||||
}
|
||||
|
||||
/// Returns the start index of the SOF sequence, if present
|
||||
fn find_sof(input: &[u8]) -> Option<usize> {
|
||||
// search for SOF
|
||||
if let Some(sof1_pos) = input.iter().position(|i| *i == Self::SOF1) {
|
||||
// drop everything before sof1_pos and check if remainder continues with SOF sequence
|
||||
let (_, msg) = input.split_at(sof1_pos);
|
||||
if msg.starts_with(&[Self::SOF1, Self::SOF2, Self::SOF3, Self::SOF4]) {
|
||||
// found the SOF sequence
|
||||
return Some(sof1_pos);
|
||||
}
|
||||
}
|
||||
|
||||
// if we reach this position, SOF sequence wasn't found
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
fn serialize_serial_msg<T>(msg: &T) -> Vec<u8>
|
||||
where
|
||||
T: prost::Message,
|
||||
{
|
||||
let mut msg_buf = msg.encode_to_vec();
|
||||
|
||||
let sof = [0x56u8, 0x32, 0x58, 0x2B];
|
||||
let eof = [0x0du8, 0x0a];
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
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);
|
||||
|
||||
out
|
||||
}
|
||||
|
||||
impl From<msg::PositionState> for crate::applogic::GnssFix {
|
||||
fn from(value: msg::PositionState) -> Self {
|
||||
let time =
|
||||
chrono::DateTime::from_timestamp_millis(value.timestamp_ms).map(|v| v.naive_utc());
|
||||
|
||||
let latitude_deg = value.position.latitude_deg.into();
|
||||
let longitude_deg = value.position.longitude_deg.into();
|
||||
let heading_deg = value.heading.map(|v| {
|
||||
#[allow(clippy::cast_precision_loss)]
|
||||
let val = (v as f32) / 10.;
|
||||
val
|
||||
});
|
||||
let speed_mps = value.speed.map(|v| {
|
||||
#[allow(clippy::cast_precision_loss)]
|
||||
let val = (v as f32) / 100.;
|
||||
val
|
||||
});
|
||||
|
||||
Self {
|
||||
time,
|
||||
latitude_deg,
|
||||
longitude_deg,
|
||||
heading_deg,
|
||||
speed_mps,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<msg::PositionState> for crate::applogic::PositionState {
|
||||
fn from(value: msg::PositionState) -> Self {
|
||||
let position = geo_types::Point::new(
|
||||
|
|
@ -49,3 +193,323 @@ impl From<msg::PositionState> for crate::applogic::PositionState {
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<msg::ItsPosition> for crate::applogic::PositionState {
|
||||
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.;
|
||||
let position = geo_types::Point::new(longitude_deg, latitude_deg);
|
||||
|
||||
Self {
|
||||
position,
|
||||
heading_deg: None,
|
||||
speed_mps: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl crate::v2x_tx::GnItsPayload for msg::RawMsgTx {
|
||||
fn make_eh(
|
||||
&self,
|
||||
address: [u8; 6],
|
||||
own_position: &crate::applogic::PositionState,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
seq_no: &mut u16,
|
||||
) -> Result<
|
||||
(
|
||||
c_its_parser::gn::en302636_4_1::ExtendedHeader,
|
||||
Option<c_its_parser::gn::en302636_4_1::AreaType>,
|
||||
u8,
|
||||
),
|
||||
alloc::string::String,
|
||||
> {
|
||||
let station_type = self
|
||||
.station_type
|
||||
.and_then(|v| {
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
let raw_val = v as u8;
|
||||
geonetworking::en302636_4_1::StationType::try_from(raw_val).ok()
|
||||
})
|
||||
.unwrap_or_default();
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
let hop_limit = self.hop_limit as u8;
|
||||
|
||||
Ok(match self.gn_transport() {
|
||||
msg::GnTransport::Gac => {
|
||||
if self.gn_area.position.is_none() {
|
||||
return Err("Failed to create GAC: gnArea.position missing".to_string());
|
||||
}
|
||||
|
||||
*seq_no += 1;
|
||||
|
||||
let (atype, geoanycast) = Self::make_geobcast(
|
||||
address,
|
||||
station_type,
|
||||
own_position,
|
||||
time,
|
||||
self.gn_area,
|
||||
*seq_no,
|
||||
)?;
|
||||
let eh = geonetworking::en302636_4_1::ExtendedHeader::GAC(geoanycast);
|
||||
(eh, Some(atype), hop_limit)
|
||||
}
|
||||
msg::GnTransport::Gbc => {
|
||||
if self.gn_area.position.is_none() {
|
||||
return Err("Failed to create GAC: gnArea.position missing".to_string());
|
||||
}
|
||||
|
||||
*seq_no += 1;
|
||||
|
||||
let (atype, geoanycast) = Self::make_geobcast(
|
||||
address,
|
||||
station_type,
|
||||
own_position,
|
||||
time,
|
||||
self.gn_area,
|
||||
*seq_no,
|
||||
)?;
|
||||
let eh = geonetworking::en302636_4_1::ExtendedHeader::GBC(geoanycast);
|
||||
(eh, Some(atype), hop_limit)
|
||||
}
|
||||
msg::GnTransport::Tsb => {
|
||||
*seq_no += 1;
|
||||
|
||||
(
|
||||
crate::v2x_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),
|
||||
None,
|
||||
1,
|
||||
),
|
||||
})
|
||||
}
|
||||
|
||||
fn make_payload(
|
||||
&self,
|
||||
) -> 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))
|
||||
}
|
||||
}
|
||||
|
||||
impl msg::RawMsgTx {
|
||||
fn make_geobcast(
|
||||
address: [u8; 6],
|
||||
station_type: geonetworking::en302636_4_1::StationType,
|
||||
own_position: &crate::applogic::PositionState,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
gn_area: msg::GnArea,
|
||||
sequence_number: u16,
|
||||
) -> Result<
|
||||
(
|
||||
c_its_parser::gn::en302636_4_1::AreaType,
|
||||
geonetworking::en302636_4_1::GeoBroadcast,
|
||||
),
|
||||
alloc::string::String,
|
||||
> {
|
||||
let source_position_vector =
|
||||
crate::v2x_tx::gn::make_lpv(address, station_type, own_position, time);
|
||||
|
||||
let (atype, dist_a, dist_b, angle) = match gn_area.area() {
|
||||
msg::GnAreaShape::EtsiAreashapeCircle => (
|
||||
c_its_parser::gn::en302636_4_1::AreaType::Circular,
|
||||
gn_area.dist_a,
|
||||
None,
|
||||
None,
|
||||
),
|
||||
msg::GnAreaShape::EtsiAreashapeRectangle => (
|
||||
c_its_parser::gn::en302636_4_1::AreaType::Rectangular,
|
||||
gn_area.dist_a,
|
||||
gn_area.dist_b,
|
||||
gn_area.angle,
|
||||
),
|
||||
msg::GnAreaShape::EtsiAreashapeEllipsis => (
|
||||
c_its_parser::gn::en302636_4_1::AreaType::Ellipsoidal,
|
||||
gn_area.dist_a,
|
||||
gn_area.dist_b,
|
||||
gn_area.angle,
|
||||
),
|
||||
};
|
||||
|
||||
#[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 distance_a = dist_a
|
||||
.map(|v| {
|
||||
#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
|
||||
let val = v as u16;
|
||||
val
|
||||
})
|
||||
.unwrap_or_default();
|
||||
let distance_b = dist_b
|
||||
.map(|v| {
|
||||
#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
|
||||
let val = v as u16;
|
||||
val
|
||||
})
|
||||
.unwrap_or_default();
|
||||
let angle = angle
|
||||
.map(|v| {
|
||||
#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
|
||||
let val = v as u16;
|
||||
val
|
||||
})
|
||||
.unwrap_or_default();
|
||||
|
||||
let gac = geonetworking::en302636_4_1::GeoAnycast::try_from_values(
|
||||
sequence_number,
|
||||
source_position_vector,
|
||||
latitude_deg,
|
||||
longitude_deg,
|
||||
distance_a,
|
||||
distance_b,
|
||||
angle,
|
||||
)
|
||||
.map_err(|err| alloc::format!("{err}"))?;
|
||||
|
||||
Ok((atype, gac))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
|
||||
use super::*;
|
||||
use crate::init_test_env_logger;
|
||||
|
||||
impl From<msg::RawMsgTx> for msg::SerialInputMsg {
|
||||
fn from(value: msg::RawMsgTx) -> Self {
|
||||
use msg::serial_input_msg::Payload;
|
||||
|
||||
Self {
|
||||
payload: Some(Payload::TxMsg(value)),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<msg::PositionState> for msg::SerialInputMsg {
|
||||
fn from(value: msg::PositionState) -> Self {
|
||||
use msg::serial_input_msg::Payload;
|
||||
|
||||
Self {
|
||||
payload: Some(Payload::Position(value)),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// manual example data:
|
||||
// SerialInputMsg(RawMsgTx): 5632582b0040123e0a340202c173256817284029d5b3400f1feaf01ffffffc23b7743e00000fc0007e8138250737feebfff600000dffff7ffff1ce40400010011807220028020d0a
|
||||
// SerialInputMsg(PositionState): 5632582b00170a1509a0bbe2a29f010000120a0d00002842150000b8410d0a
|
||||
|
||||
#[test]
|
||||
fn uart_parser() {
|
||||
// generate some test UART input messages
|
||||
let raw_tx = msg::RawMsgTx {
|
||||
payload: crate::testdata::CAM_SMALL.to_vec(),
|
||||
message_type: 1,
|
||||
gn_transport: msg::GnTransport::Shb.into(),
|
||||
gn_area: msg::GnArea {
|
||||
area: None,
|
||||
position: None,
|
||||
dist_a: None,
|
||||
dist_b: None,
|
||||
angle: None,
|
||||
},
|
||||
hop_limit: 2,
|
||||
station_type: None,
|
||||
};
|
||||
let raw_tx_buf = serialize_serial_msg::<msg::SerialInputMsg>(&raw_tx.clone().into());
|
||||
|
||||
let pos_state = msg::PositionState {
|
||||
timestamp_ms: 1785144196000,
|
||||
position: msg::Position {
|
||||
latitude_deg: 42.,
|
||||
longitude_deg: 23.,
|
||||
},
|
||||
heading: None,
|
||||
speed: None,
|
||||
};
|
||||
let pos_state_buf = serialize_serial_msg::<msg::SerialInputMsg>(&pos_state.clone().into());
|
||||
|
||||
let mut parser = Parser::default();
|
||||
|
||||
// feed individual complete messages to parser
|
||||
let output = parser.parse(&raw_tx_buf);
|
||||
assert_eq!(1, output.len());
|
||||
assert_eq!(
|
||||
&msg::SerialInputMsg::from(raw_tx.clone()),
|
||||
output.get(0).unwrap()
|
||||
);
|
||||
|
||||
let output = parser.parse(&pos_state_buf);
|
||||
assert_eq!(1, output.len());
|
||||
assert_eq!(
|
||||
&msg::SerialInputMsg::from(pos_state.clone()),
|
||||
output.get(0).unwrap()
|
||||
);
|
||||
|
||||
// feed multiple complete messages to parser
|
||||
let mut combined_buf = pos_state_buf.clone();
|
||||
combined_buf.extend_from_slice(&raw_tx_buf);
|
||||
|
||||
let output = parser.parse(&combined_buf);
|
||||
assert_eq!(2, output.len());
|
||||
assert_eq!(
|
||||
&msg::SerialInputMsg::from(pos_state.clone()),
|
||||
output.get(0).unwrap()
|
||||
);
|
||||
assert_eq!(
|
||||
&msg::SerialInputMsg::from(raw_tx.clone()),
|
||||
output.get(1).unwrap()
|
||||
);
|
||||
|
||||
// feed partial messages to parser
|
||||
let buf1 = &raw_tx_buf[0..3];
|
||||
let buf2 = &raw_tx_buf[3..];
|
||||
|
||||
let output = parser.parse(buf1);
|
||||
assert_eq!(0, output.len());
|
||||
|
||||
let output = parser.parse(buf2);
|
||||
assert_eq!(1, output.len());
|
||||
assert_eq!(
|
||||
&msg::SerialInputMsg::from(raw_tx.clone()),
|
||||
output.get(0).unwrap()
|
||||
);
|
||||
|
||||
// feed messages with garbage in between
|
||||
let mut buf1 = vec![0xabu8, 0xcd]; // prepend some garbage
|
||||
buf1.extend_from_slice(&raw_tx_buf[0..3]);
|
||||
let mut buf2 = (&raw_tx_buf[3..]).to_vec();
|
||||
buf2.extend_from_slice(&[0xfeu8, 0x42]); // append some garbage
|
||||
buf2.extend_from_slice(&pos_state_buf); // append second message
|
||||
buf2.extend_from_slice(&[0x11u8, 0x22]); // append some more garbage
|
||||
|
||||
let output = parser.parse(&buf1);
|
||||
assert_eq!(0, output.len());
|
||||
|
||||
let output = parser.parse(&buf2);
|
||||
assert_eq!(2, output.len());
|
||||
assert_eq!(
|
||||
&msg::SerialInputMsg::from(raw_tx.clone()),
|
||||
output.get(0).unwrap()
|
||||
);
|
||||
assert_eq!(
|
||||
&msg::SerialInputMsg::from(pos_state.clone()),
|
||||
output.get(1).unwrap()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
114
src/main.rs
114
src/main.rs
|
|
@ -7,6 +7,13 @@
|
|||
)]
|
||||
// #![deny(clippy::large_stack_frames)]
|
||||
|
||||
// we can't use UART I/O and GNSS at the same time
|
||||
#[cfg(all(feature = "uart", feature = "gnss"))]
|
||||
compile_error!("feature \"uart\" and feature \"gnss\" cannot be enabled at the same time");
|
||||
// autonomous CAM generation need a time and position source (aka _gnss)
|
||||
#[cfg(all(feature = "cam_tx", not(feature = "_gnss")))]
|
||||
compile_error!("feature \"cam_tx\" needs a time and position source (either GNSS or UART)");
|
||||
|
||||
use alloc::string::ToString as _;
|
||||
use alloc::vec::Vec;
|
||||
use core::cell::RefCell;
|
||||
|
|
@ -35,17 +42,18 @@ extern crate alloc;
|
|||
mod applogic;
|
||||
mod cache;
|
||||
mod geo_alg;
|
||||
#[cfg(feature = "uart")]
|
||||
mod io;
|
||||
mod radio;
|
||||
#[cfg(feature = "screen")]
|
||||
mod screen;
|
||||
#[cfg(feature = "cam_tx")]
|
||||
#[cfg(any(feature = "cam_tx", feature = "uart"))]
|
||||
mod v2x_tx;
|
||||
|
||||
const WIFI_CHANNEL: radio::Channel = 180;
|
||||
#[cfg(feature = "spat")]
|
||||
const SPAT_RATE_LIMIT: u8 = 5; // keep every n-th message
|
||||
#[cfg(feature = "cam_tx")]
|
||||
#[cfg(any(feature = "cam_tx", feature = "uart"))]
|
||||
const GN_IS_MOBILE: bool = true;
|
||||
#[cfg(feature = "cam_tx")]
|
||||
const OWN_STATION_TYPE: ItsStationType = ItsStationType::Cyclist;
|
||||
|
|
@ -60,7 +68,7 @@ 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!();
|
||||
|
||||
#[cfg(feature = "gnss")]
|
||||
#[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));
|
||||
#[cfg(feature = "_uart")]
|
||||
|
|
@ -136,6 +144,18 @@ async fn main(spawner: Spawner) -> ! {
|
|||
screen
|
||||
};
|
||||
|
||||
// Setup UART I/O channel
|
||||
#[cfg(feature = "uart")]
|
||||
setup_uart0(
|
||||
115_200,
|
||||
Some(b'\n'),
|
||||
peripherals.GPIO12,
|
||||
peripherals.GPIO11,
|
||||
peripherals.UART0,
|
||||
serial_handler,
|
||||
)
|
||||
.expect("Failed to initialize UART");
|
||||
|
||||
// Setup WiFi
|
||||
// sadly, it doesn't work any more when we move `esp_radio::wifi::new` to `setup_wifi_sniffer`
|
||||
let (mut wifi_controller, interfaces) = esp_radio::wifi::new(
|
||||
|
|
@ -178,24 +198,29 @@ async fn main(spawner: Spawner) -> ! {
|
|||
#[cfg(feature = "gnss")]
|
||||
info!("GNSS task started, waiting for GNSS fix...");
|
||||
|
||||
let mut state = applogic::State::new();
|
||||
#[cfg(feature = "cam_tx")]
|
||||
let (mut seq_no, mut cam, mac_bytes) = {
|
||||
let station_id = v2x_tx::make_station_id(mac);
|
||||
info!("V2X Station ID: {station_id}");
|
||||
|
||||
#[cfg(any(feature = "cam_tx", feature = "uart"))]
|
||||
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();
|
||||
|
||||
let cam = applogic::cam_tx::CamState::new(
|
||||
(0, mac_bytes)
|
||||
};
|
||||
|
||||
let mut state = applogic::State::new();
|
||||
#[cfg(feature = "cam_tx")]
|
||||
let mut cam = {
|
||||
let station_id = v2x_tx::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,
|
||||
);
|
||||
|
||||
(0, cam, mac_bytes)
|
||||
)
|
||||
};
|
||||
#[cfg(feature = "uart")]
|
||||
let mut uart_input = io::Parser::default();
|
||||
loop {
|
||||
embassy_time::Timer::after(embassy_time::Duration::from_millis(1)).await;
|
||||
|
||||
|
|
@ -214,11 +239,56 @@ async fn main(spawner: Spawner) -> ! {
|
|||
});
|
||||
|
||||
if let Some(data) = new_data {
|
||||
warn!("TODO: New UART data: {data:x?}");
|
||||
#[cfg(feature = "uart")]
|
||||
{
|
||||
for msg in uart_input.parse(&data) {
|
||||
match msg.payload {
|
||||
Some(io::msg::serial_input_msg::Payload::Position(fix)) => {
|
||||
let pos_state = fix.into();
|
||||
|
||||
critical_section::with(|cs| {
|
||||
let gnss_update_ref = GNSS_UPDATE.borrow(cs);
|
||||
gnss_update_ref.replace(Some(pos_state));
|
||||
});
|
||||
}
|
||||
Some(io::msg::serial_input_msg::Payload::TxMsg(tx_request)) => {
|
||||
use v2x_tx::GnItsPayload as _;
|
||||
|
||||
info!(
|
||||
"UART: Got new TX message: Type {} with {} bytes",
|
||||
tx_request.message_type,
|
||||
tx_request.payload.len()
|
||||
);
|
||||
let own_position = state.pos();
|
||||
let time = state.time().and_utc();
|
||||
|
||||
match tx_request.encode_packet(
|
||||
mac_bytes,
|
||||
own_position,
|
||||
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 GN message: {err}"),
|
||||
}
|
||||
}
|
||||
None => {}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "gnss")]
|
||||
#[cfg(feature = "_gnss")]
|
||||
{
|
||||
let mut new_position = false;
|
||||
critical_section::with(|cs| {
|
||||
|
|
@ -245,7 +315,7 @@ async fn main(spawner: Spawner) -> ! {
|
|||
Ok(data) => {
|
||||
if let Err(err) = radio::send_80211_bcast_frame(
|
||||
&mut wlan_iface,
|
||||
mac.as_bytes(),
|
||||
&mac_bytes.clone(),
|
||||
&data,
|
||||
) {
|
||||
warn!("Failed to send 802.11 frame: {err}");
|
||||
|
|
@ -472,6 +542,18 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
|
|||
error!("V2X RX queue is full");
|
||||
}
|
||||
});
|
||||
|
||||
// Send data to UART
|
||||
#[cfg(feature = "uart")]
|
||||
{
|
||||
let message = io::msg::RawMsgRx {
|
||||
msg_type: message_id.as_u8().into(),
|
||||
payload: frame.data.to_vec(),
|
||||
}
|
||||
.serialize_uart_proto();
|
||||
|
||||
send_uart0(&message);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@ extern crate alloc;
|
|||
mod applogic;
|
||||
mod cache;
|
||||
mod geo_alg;
|
||||
mod io;
|
||||
mod radio;
|
||||
mod testdata;
|
||||
mod v2x_tx;
|
||||
|
|
|
|||
|
|
@ -455,3 +455,11 @@ pub const CAM: &[u8] = &[
|
|||
0x00, 0x06, 0xc6, 0x70, 0x00, 0xdf, 0x80, 0x8d, 0x5f, 0xde, 0x66, 0x27, 0x00, 0x07, 0x3c, 0x04,
|
||||
0x76, 0xfd, 0x67, 0x31, 0x9c, 0x00, 0x58, 0x60, 0x41, 0x37, 0xd3, 0x15, 0x89, 0xc0, 0x06, 0xdc,
|
||||
];
|
||||
|
||||
#[cfg(any(feature = "cam_tx", feature = "uart"))]
|
||||
pub const CAM_SMALL: &[u8] = &[
|
||||
0x2, 0x2, 0xc1, 0x73, 0x25, 0x68, 0x17, 0x28, 0x40, 0x29, 0xd5, 0xb3, 0x40, 0xf, 0x1f, 0xea,
|
||||
0xf0, 0x1f, 0xff, 0xff, 0xfc, 0x23, 0xb7, 0x74, 0x3e, 0x0, 0x0, 0xf, 0xc0, 0x0, 0x7e, 0x81,
|
||||
0x38, 0x25, 0x7, 0x37, 0xfe, 0xeb, 0xff, 0xf6, 0x0, 0x0, 0xd, 0xff, 0xff, 0x7f, 0xff, 0xf1,
|
||||
0xce, 0x40, 0x40, 0x0,
|
||||
];
|
||||
|
|
|
|||
|
|
@ -67,7 +67,7 @@ pub fn make_gbc_eh(
|
|||
)
|
||||
}
|
||||
|
||||
fn make_lpv(
|
||||
pub fn make_lpv(
|
||||
address: [u8; 6],
|
||||
station_type: geonetworking::en302636_4_1::StationType,
|
||||
own_position: &crate::applogic::PositionState,
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@ use c_its_parser::gn::{self as geonetworking, Encode};
|
|||
|
||||
pub mod gn;
|
||||
|
||||
#[cfg(feature = "esp")]
|
||||
#[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.
|
||||
|
|
@ -40,11 +40,14 @@ pub trait GnItsPayload {
|
|||
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(
|
||||
|
|
@ -74,7 +77,8 @@ pub trait GnItsPayload {
|
|||
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);
|
||||
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(
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue