Enable V2X transmission, add optional stand-alone CAM transmission
This commit is contained in:
Jannik Beyerstedt
parent
1d4e82335d
commit
2379a7eefc
9 changed files with 543 additions and 14 deletions
197
src/applogic/cam_tx.rs
Normal file
197
src/applogic/cam_tx.rs
Normal file
|
|
@ -0,0 +1,197 @@
|
|||
//! C-ITS CAM Transmission
|
||||
|
||||
use alloc::vec::Vec;
|
||||
|
||||
use c_its_parser::gn as geonetworking;
|
||||
use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions;
|
||||
use c_its_parser::standards::cdd_1_3_1_1::its_container;
|
||||
use c_its_parser::standards::extensions;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct CamState {
|
||||
station_id: u32,
|
||||
station_type: its_container::StationType,
|
||||
|
||||
vehicle_length_dm: u16, // vehicle length is in 10cm steps
|
||||
vehicle_width_dm: u8, // vehicle width is in 10cm steps
|
||||
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
pos_state: super::PositionState,
|
||||
}
|
||||
|
||||
impl CamState {
|
||||
pub fn new(
|
||||
station_id: u32,
|
||||
station_type: c_its_parser::standards::extensions::ItsStationType,
|
||||
vehicle_length_dm: u16,
|
||||
vehicle_width_dm: u8,
|
||||
) -> Self {
|
||||
Self {
|
||||
station_id,
|
||||
station_type: station_type.into(),
|
||||
vehicle_length_dm,
|
||||
vehicle_width_dm,
|
||||
time: chrono::DateTime::<chrono::Utc>::default(),
|
||||
pos_state: super::PositionState::default(),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn update(&mut self, time: chrono::DateTime<chrono::Utc>, pos: super::PositionState) {
|
||||
self.time = time;
|
||||
self.pos_state = pos;
|
||||
|
||||
// TODO: build CAM trace
|
||||
}
|
||||
|
||||
#[allow(clippy::too_many_lines)]
|
||||
fn make_cam(&self) -> Result<cam_pdu_descriptions::CAM, alloc::string::String> {
|
||||
let header = its_container::ItsPduHeader::new(
|
||||
2,
|
||||
extensions::ItsMessageId::Cam.as_u8(),
|
||||
its_container::StationID(self.station_id),
|
||||
);
|
||||
|
||||
let generation_delta_time = {
|
||||
let its_timestamp = c_its_parser::time_utils::TimestampIts::from(self.time);
|
||||
|
||||
#[allow(clippy::cast_possible_truncation)]
|
||||
let ts_mod = (its_timestamp.0 % 65_536) as u16;
|
||||
|
||||
cam_pdu_descriptions::GenerationDeltaTime(ts_mod)
|
||||
};
|
||||
|
||||
let heading = match self.pos_state.heading_deg {
|
||||
Some(value) => its_container::Heading {
|
||||
heading_value: its_container::HeadingValue::from_deg(value)
|
||||
.map_err(|err| alloc::format!("Failed to make HeadingValue: {err}"))?,
|
||||
heading_confidence: its_container::HeadingConfidence(127), // unavailable for now
|
||||
},
|
||||
None => its_container::Heading {
|
||||
heading_value: its_container::HeadingValue(0),
|
||||
heading_confidence: its_container::HeadingConfidence(127), // unavailable for now
|
||||
},
|
||||
};
|
||||
let speed = match self.pos_state.speed_mps {
|
||||
Some(value) => its_container::Speed {
|
||||
speed_value: its_container::SpeedValue::from_mps(value)
|
||||
.map_err(|err| alloc::format!("Failed to make SpeedValue: {err}"))?,
|
||||
speed_confidence: its_container::SpeedConfidence(127), // unavailable for now
|
||||
},
|
||||
None => its_container::Speed {
|
||||
speed_value: its_container::SpeedValue(0),
|
||||
speed_confidence: its_container::SpeedConfidence(127), // unavailable for now
|
||||
},
|
||||
};
|
||||
let reference_position = its_container::ReferencePosition {
|
||||
latitude: its_container::Latitude::from_deg(self.pos_state.position.y()),
|
||||
longitude: its_container::Longitude::from_deg(self.pos_state.position.x()),
|
||||
position_confidence_ellipse: its_container::PosConfidenceEllipse {
|
||||
semi_major_confidence: its_container::SemiAxisLength(4095), // unavailable for now
|
||||
semi_minor_confidence: its_container::SemiAxisLength(4095), // unavailable for now
|
||||
semi_major_orientation: its_container::HeadingValue::unavailable(),
|
||||
},
|
||||
altitude: its_container::Altitude {
|
||||
altitude_value: its_container::AltitudeValue(800_001), // unavailable for now
|
||||
altitude_confidence: its_container::AltitudeConfidence::unavailable,
|
||||
},
|
||||
};
|
||||
|
||||
let basic_container = cam_pdu_descriptions::BasicContainer::new(
|
||||
self.station_type.clone(),
|
||||
reference_position,
|
||||
);
|
||||
let high_frequency_container =
|
||||
cam_pdu_descriptions::HighFrequencyContainer::basicVehicleContainerHighFrequency(
|
||||
cam_pdu_descriptions::BasicVehicleContainerHighFrequency {
|
||||
heading,
|
||||
speed,
|
||||
drive_direction: its_container::DriveDirection::unavailable,
|
||||
vehicle_length: its_container::VehicleLength {
|
||||
vehicle_length_value: its_container::VehicleLengthValue(
|
||||
self.vehicle_length_dm,
|
||||
),
|
||||
vehicle_length_confidence_indication:
|
||||
its_container::VehicleLengthConfidenceIndication::unavailable,
|
||||
},
|
||||
vehicle_width: its_container::VehicleWidth(self.vehicle_width_dm),
|
||||
longitudinal_acceleration: its_container::LongitudinalAcceleration {
|
||||
longitudinal_acceleration_value:
|
||||
its_container::LongitudinalAccelerationValue::unavailable(),
|
||||
longitudinal_acceleration_confidence: its_container::AccelerationConfidence(
|
||||
102, // unavailable
|
||||
),
|
||||
},
|
||||
curvature: its_container::Curvature {
|
||||
curvature_value: its_container::CurvatureValue::unavailable(),
|
||||
curvature_confidence: its_container::CurvatureConfidence::unavailable,
|
||||
},
|
||||
curvature_calculation_mode:
|
||||
its_container::CurvatureCalculationMode::unavailable,
|
||||
yaw_rate: its_container::YawRate {
|
||||
yaw_rate_value: its_container::YawRateValue::unavailable(),
|
||||
yaw_rate_confidence: its_container::YawRateConfidence::unavailable,
|
||||
},
|
||||
acceleration_control: None,
|
||||
lane_position: None,
|
||||
steering_wheel_angle: None,
|
||||
lateral_acceleration: None,
|
||||
vertical_acceleration: None,
|
||||
performance_class: None,
|
||||
cen_dsrc_tolling_zone: None,
|
||||
},
|
||||
);
|
||||
let low_frequency_container =
|
||||
cam_pdu_descriptions::LowFrequencyContainer::basicVehicleContainerLowFrequency(
|
||||
cam_pdu_descriptions::BasicVehicleContainerLowFrequency {
|
||||
vehicle_role: its_container::VehicleRole::default,
|
||||
exterior_lights: its_container::ExteriorLights::default(),
|
||||
path_history: its_container::PathHistory(alloc::vec![]), // TODO: fill trace
|
||||
},
|
||||
);
|
||||
|
||||
let cam_parameters = cam_pdu_descriptions::CamParameters::new(
|
||||
basic_container,
|
||||
high_frequency_container,
|
||||
Some(low_frequency_container),
|
||||
None,
|
||||
);
|
||||
|
||||
Ok(cam_pdu_descriptions::CAM::new(
|
||||
header,
|
||||
cam_pdu_descriptions::CoopAwareness::new(generation_delta_time, cam_parameters),
|
||||
))
|
||||
}
|
||||
}
|
||||
|
||||
impl crate::v2x_tx::GnItsPayload for CamState {
|
||||
fn make_eh(
|
||||
&self,
|
||||
address: [u8; 6],
|
||||
own_position: &crate::applogic::PositionState,
|
||||
time: chrono::DateTime<chrono::Utc>,
|
||||
_seq_no: &mut u16,
|
||||
) -> (
|
||||
geonetworking::en302636_4_1::ExtendedHeader,
|
||||
Option<geonetworking::en302636_4_1::AreaType>,
|
||||
u8,
|
||||
) {
|
||||
let station_type = geonetworking::en302636_4_1::StationType::try_from(self.station_type.0)
|
||||
.unwrap_or_default();
|
||||
|
||||
// CAM always uses SHB, hop-limit 1
|
||||
(
|
||||
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>
|
||||
{
|
||||
let msg = self.make_cam()?;
|
||||
let uper = msg.encode_to_uper()?;
|
||||
Ok((uper, c_its_parser::standards::extensions::ItsMessageId::Cam))
|
||||
}
|
||||
}
|
||||
|
|
@ -18,6 +18,8 @@ pub mod v2x;
|
|||
|
||||
#[cfg(feature = "cam")]
|
||||
pub mod cam;
|
||||
#[cfg(feature = "cam_tx")]
|
||||
pub mod cam_tx;
|
||||
#[cfg(feature = "denm")]
|
||||
pub mod denm;
|
||||
#[cfg(feature = "spat")]
|
||||
|
|
|
|||
Loading…
Reference in a new issue