722 lines
23 KiB
Rust
722 lines
23 KiB
Rust
//! 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"));
|
|
}
|
|
|
|
use alloc::vec::Vec;
|
|
|
|
#[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) -> Vec<u8> {
|
|
serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(self.clone()))
|
|
}
|
|
}
|
|
impl msg::DenmEvent {
|
|
#[cfg(feature = "uart")]
|
|
#[allow(unused)]
|
|
pub fn serialize_uart_proto(&self) -> Vec<u8> {
|
|
serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(*self))
|
|
}
|
|
|
|
#[cfg(feature = "denm")]
|
|
pub fn serialize_ble_proto(&self) -> Vec<u8> {
|
|
use prost::Message as _;
|
|
|
|
self.encode_to_vec()
|
|
}
|
|
}
|
|
impl msg::CamEvent {
|
|
#[cfg(feature = "cam")]
|
|
pub fn serialize_ble_proto(&self) -> Vec<u8> {
|
|
use prost::Message as _;
|
|
|
|
self.encode_to_vec()
|
|
}
|
|
}
|
|
|
|
#[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) -> 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(feature = "uart")]
|
|
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<(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
|
|
.into();
|
|
|
|
let vehicle_role = value
|
|
.cam
|
|
.cam_parameters
|
|
.low_frequency_container
|
|
.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
|
|
.and_then(|v| v.event_position_heading)
|
|
.map(|v| {
|
|
#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
|
|
let conv = v.value.as_deg() as u32;
|
|
conv
|
|
});
|
|
let direction = denm_mgmt
|
|
.traffic_direction
|
|
.map(|v| msg::TrafficDir::from(v) as i32);
|
|
let cc_tuple = value
|
|
.denm
|
|
.situation
|
|
.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,
|
|
}
|
|
}
|
|
}
|
|
|
|
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,
|
|
}
|
|
}
|
|
}
|
|
|
|
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,
|
|
}
|
|
}
|
|
}
|
|
|
|
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(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
|
|
|
|
#[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()
|
|
);
|
|
}
|
|
}
|