265f2e4eea
BLE can only fit 251 bytes of payload in notifications which isn't nearly enough for a raw V2X message. So only publish a notification which message was received and let the client poll the content from the characteristic (since reads can get data in fragments)
849 lines
27 KiB
Rust
849 lines
27 KiB
Rust
//! UART and 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 c_its_parser::gn as geonetworking;
|
|
|
|
#[cfg(feature = "uart")]
|
|
impl From<msg::RawMsgRx> for msg::SerialOutputMsg {
|
|
fn from(value: msg::RawMsgRx) -> Self {
|
|
use msg::serial_output_msg::Payload;
|
|
|
|
Self {
|
|
payload: Some(Payload::RxMsg(value)),
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "uart")]
|
|
impl From<msg::DenmEvent> for msg::SerialOutputMsg {
|
|
fn from(value: msg::DenmEvent) -> Self {
|
|
use msg::serial_output_msg::Payload;
|
|
|
|
Self {
|
|
payload: Some(Payload::Denm(value)),
|
|
}
|
|
}
|
|
}
|
|
|
|
impl msg::RawMsgRx {
|
|
#[cfg(feature = "uart")]
|
|
pub fn serialize_uart_proto(&self) -> alloc::vec::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)
|
|
}
|
|
}
|
|
}
|
|
|
|
#[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;
|
|
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) => log::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
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "uart")]
|
|
fn serialize_serial_msg<T>(msg: &T) -> alloc::vec::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
|
|
}
|
|
|
|
#[cfg(any(feature = "uart", feature = "ble"))]
|
|
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,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "uart")]
|
|
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(),
|
|
value.position.latitude_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 {
|
|
position,
|
|
heading_deg,
|
|
speed_mps,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[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.;
|
|
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,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "uart")]
|
|
impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx {
|
|
fn make_eh(
|
|
&self,
|
|
address: [u8; 6],
|
|
own_position: &esp32_cits_core::PosVel,
|
|
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,
|
|
> {
|
|
use alloc::string::ToString as _;
|
|
|
|
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;
|
|
|
|
(
|
|
esp32_cits_core::tx::gn::make_tsb_eh(
|
|
address,
|
|
station_type,
|
|
own_position,
|
|
time,
|
|
*seq_no,
|
|
)?,
|
|
None,
|
|
hop_limit,
|
|
)
|
|
}
|
|
msg::GnTransport::Shb => (
|
|
esp32_cits_core::tx::gn::make_shb_eh(address, station_type, own_position, time)?,
|
|
None,
|
|
1,
|
|
),
|
|
})
|
|
}
|
|
|
|
fn make_payload(
|
|
&self,
|
|
) -> Result<
|
|
(
|
|
alloc::vec::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],
|
|
station_type: geonetworking::en302636_4_1::StationType,
|
|
own_position: &esp32_cits_core::PosVel,
|
|
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 =
|
|
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 => (
|
|
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(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;
|
|
|
|
Self {
|
|
payload: Some(Payload::TxMsg(value)),
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "uart")]
|
|
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
|
|
// PositionState: 09a0bbe2a29f010000120a0d00002842150000b841
|
|
|
|
#[cfg(feature = "uart")]
|
|
#[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()
|
|
);
|
|
}
|
|
}
|