From 7bbb47a844d96195abf2075eb388bc2faa9ef088 Mon Sep 17 00:00:00 2001 From: Jannik Beyerstedt Date: Fri, 24 Jul 2026 17:25:52 +0200 Subject: [PATCH] cam_tx: Fill path history --- Cargo.toml | 2 +- feat-permut.sh | 1 - src/applogic/cam_tx.rs | 230 ++++++++++++++++++++++++++++++++++++++++- src/applogic/mod.rs | 2 +- src/test.rs | 1 + 5 files changed, 231 insertions(+), 5 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 834f164..fb0d588 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -12,7 +12,7 @@ path = "./src/main.rs" [[bin]] name = "test" path = "./src/test.rs" -required-features = ["std"] +required-features = ["std", "cam"] [features] default = ["esp", "spat", "gnss", "screen"] diff --git a/feat-permut.sh b/feat-permut.sh index a40a8ca..6ddd438 100755 --- a/feat-permut.sh +++ b/feat-permut.sh @@ -17,7 +17,6 @@ features_more=( spat,gnss,screen,cam_tx,cam,denm ) features_test=( - spat spat,cam,denm ) diff --git a/src/applogic/cam_tx.rs b/src/applogic/cam_tx.rs index 7de554e..0cd0a16 100644 --- a/src/applogic/cam_tx.rs +++ b/src/applogic/cam_tx.rs @@ -7,6 +7,10 @@ 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; +const PH_MAX_LENGHT_M: f32 = 500.; +const PH_MAX_ITEMS: usize = 23; +const PH_MIN_DIST_M: f32 = 20.; + #[derive(Debug)] pub struct CamState { station_id: u32, @@ -17,6 +21,39 @@ pub struct CamState { time: chrono::DateTime, pos_state: super::PositionState, + + /// database for path history, oldest item at the back of the vector + path_history: Vec, +} + +#[derive(Debug, Clone, Copy, PartialEq)] +struct PathPoint { + pos: geo_types::Point, + time: chrono::DateTime, + /// distance to previous point. Ignore on last point! + dist_m: f32, +} + +impl PathPoint { + fn new_from_pos(pos: &super::PositionState, time: chrono::DateTime) -> Self { + Self { + pos: pos.position, + time, + dist_m: 0., + } + } + + fn new_from_pos_with_dist( + pos: &super::PositionState, + time: chrono::DateTime, + dist_m: f32, + ) -> Self { + Self { + pos: pos.position, + time, + dist_m, + } + } } impl CamState { @@ -33,6 +70,7 @@ impl CamState { vehicle_width_dm, time: chrono::DateTime::::default(), pos_state: super::PositionState::default(), + path_history: Vec::with_capacity(PH_MAX_ITEMS + 1), } } @@ -40,7 +78,99 @@ impl CamState { self.time = time; self.pos_state = pos; - // TODO: build CAM trace + // Update path history + Self::update_ph(&mut self.path_history, time, &self.pos_state); + } + + // this is a function, not a method for easier testing + fn update_ph( + path_history: &mut Vec, + time: chrono::DateTime, + pos: &super::PositionState, + ) { + // Update path history + if path_history.is_empty() { + // prime with current position if empty + path_history.push(PathPoint::new_from_pos(pos, time)); + } else { + // unwrap is fine since we checked for non-empty before + let prev = path_history.first().unwrap(); + + // only add a point, if we moved more than PH_MIN_DIST_M + #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)] + let dist = crate::geo_alg::haversine_dist(&prev.pos, &pos.position) as f32; + if dist > PH_MIN_DIST_M { + // new items shall be at the front of the vector + path_history.insert(0, PathPoint::new_from_pos_with_dist(pos, time, dist)); + } + } + + // check length (PH_MAX_LENGHT_M and PH_MAX_ITEMS points max) + if path_history.len() > PH_MAX_ITEMS { + let _ = path_history.pop(); + } + + let (_, cutoff_idx) = + path_history + .iter() + .enumerate() + .fold((0., None), |(len, cutoff_idx), (idx, item)| { + if cutoff_idx.is_none() { + let new_len = len + item.dist_m; + let new_cutoff_idx = if len > PH_MAX_LENGHT_M { + Some(idx) + } else { + None + }; + + (new_len, new_cutoff_idx) + } else { + (len, cutoff_idx) + } + }); + if let Some(idx) = cutoff_idx { + path_history.drain(idx..); + } + } + + fn make_ph(&self) -> Vec { + let mut ph = alloc::vec![]; + + self.path_history.iter().fold( + PathPoint::new_from_pos(&self.pos_state, self.time), + |prev_point, pt| { + // calculate delta position and append to `ph` + + let delta_time = pt.time - prev_point.time; + let delta_lat = pt.pos.y() - prev_point.pos.y(); + let delta_lon = pt.pos.x() - prev_point.pos.x(); + + // delta time is in 1/100 second / 10 ms steps + let path_delta_time = its_container::PathDeltaTime( + (delta_time.num_milliseconds().abs() / 10) + .min(65_535) // clamp to max value + .into(), + ); + let delta_latitude = its_container::DeltaLatitude::from_deg(delta_lat); + let delta_longitude = its_container::DeltaLongitude::from_deg(delta_lon); + let delta_altitude = its_container::DeltaAltitude::unavailable(); + + let ppoint = its_container::PathPoint::new( + its_container::DeltaReferencePosition::new( + delta_latitude, + delta_longitude, + delta_altitude, + ), + Some(path_delta_time), + ); + + ph.push(ppoint); + + *pt + }, + ); + + ph } #[allow(clippy::too_many_lines)] @@ -145,7 +275,7 @@ impl CamState { 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 + path_history: its_container::PathHistory(self.make_ph()), }, ); @@ -195,3 +325,99 @@ impl crate::v2x_tx::GnItsPayload for CamState { Ok((uper, c_its_parser::standards::extensions::ItsMessageId::Cam)) } } + +#[cfg(test)] +mod tests { + use alloc::vec; + + use super::*; + use crate::init_test_env_logger; + + fn posstate_from_pos(position: geo_types::Point) -> crate::applogic::PositionState { + crate::applogic::PositionState { + position, + heading_deg: None, + speed_mps: None, + } + } + + #[test] + fn cam_path_history_update() { + init_test_env_logger(); + + let time = chrono::DateTime::from_timestamp(0, 0).unwrap(); + let p1 = geo_types::Point::new(10., 53.5); + let p2 = geo_types::Point::new(10., 53.5 + 0.000045); // ~5m further (< PH_MIN_DIST_M) + let p3 = geo_types::Point::new(10., 53.5 + 0.00027); // ~30m further (> PH_MIN_DIST_M) + + let ps1 = posstate_from_pos(p1); + let pp1 = PathPoint::new_from_pos(&ps1, time); + let ps2 = posstate_from_pos(p2); + let pp2 = PathPoint::new_from_pos(&ps2, time); + let ps3 = posstate_from_pos(p3); + let pp3 = PathPoint::new_from_pos_with_dist(&ps3, time, 30.02267); + + let mut path_history: Vec = vec![]; + + // add first point, expect one point in PH + CamState::update_ph(&mut path_history, time, &posstate_from_pos(p1)); + assert_eq!(1, path_history.len()); + assert_eq!(&pp1, path_history.first().unwrap()); + + // add insignificant point, expect still one point in PH + CamState::update_ph(&mut path_history, time, &posstate_from_pos(p2)); + assert_eq!(1, path_history.len()); + assert_eq!(&pp1, path_history.first().unwrap()); + + // add significantly moved point, expect [pp3, pp1] vector + CamState::update_ph(&mut path_history, time, &posstate_from_pos(p3)); + assert_eq!(2, path_history.len()); + assert_eq!(&pp3, path_history.first().unwrap()); + assert_eq!(&pp1, path_history.last().unwrap()); + } + + #[test] + fn cam_path_history_max_len() { + init_test_env_logger(); + + let time = chrono::DateTime::from_timestamp(0, 0).unwrap(); + let p1 = geo_types::Point::new(10., 53.5); + let p2 = geo_types::Point::new(10., 53.5 + 0.00023); // at ~25m from p1 (> PH_MIN_DIST_M) + let p3 = geo_types::Point::new(10., 53.5 + 0.00445); // at ~495m from p1 (slightly less than PH_MAX_LENGHT_M) + let p4 = geo_types::Point::new(10., 53.5 + 0.00477); // at ~530m from p1 (slightly more than PH_MAX_LENGHT_M) + + let ps1 = posstate_from_pos(p1); + let pp1 = PathPoint::new_from_pos(&ps1, time); + let ps2 = posstate_from_pos(p2); + let pp2 = PathPoint::new_from_pos(&ps2, time); + let ps3 = posstate_from_pos(p3); + let pp3 = PathPoint::new_from_pos(&ps3, time); + let ps4 = posstate_from_pos(p4); + let pp4 = PathPoint::new_from_pos(&ps4, time); + + let mut path_history: Vec = vec![]; + + // prime with p3, p4 (should give a length of ~470m) + CamState::update_ph(&mut path_history, time, &posstate_from_pos(p4)); + CamState::update_ph(&mut path_history, time, &posstate_from_pos(p3)); + assert_eq!(2, path_history.len()); + + // Add p2, should drop p4 from vector because of > PH_MAX_LENGHT_M + CamState::update_ph(&mut path_history, time, &posstate_from_pos(p2)); + assert_eq!(2, path_history.len()); + assert_eq!(p2, path_history.get(0).unwrap().pos); + assert_eq!(469.24323, path_history.get(0).unwrap().dist_m); + assert_eq!(p3, path_history.get(1).unwrap().pos); + assert_eq!(35.582424, path_history.get(1).unwrap().dist_m); + + // Add p1, should not drop anything from vector because still < PH_MAX_LENGHT_M + CamState::update_ph(&mut path_history, time, &posstate_from_pos(p1)); + assert_eq!(3, path_history.len()); + assert_eq!(p1, path_history.get(0).unwrap().pos); + assert_eq!(25.57487, path_history.get(0).unwrap().dist_m); + assert_eq!(p2, path_history.get(1).unwrap().pos); + assert_eq!(469.24323, path_history.get(1).unwrap().dist_m); + assert_eq!(p3, path_history.get(2).unwrap().pos); + assert_eq!(35.582424, path_history.get(2).unwrap().dist_m); + } +} diff --git a/src/applogic/mod.rs b/src/applogic/mod.rs index d1d082e..32ee4f6 100644 --- a/src/applogic/mod.rs +++ b/src/applogic/mod.rs @@ -18,7 +18,7 @@ pub mod v2x; #[cfg(feature = "cam")] pub mod cam; -#[cfg(feature = "cam_tx")] +#[cfg(any(feature = "cam_tx", test))] pub mod cam_tx; #[cfg(feature = "denm")] pub mod denm; diff --git a/src/test.rs b/src/test.rs index 5a04150..7de1bf8 100644 --- a/src/test.rs +++ b/src/test.rs @@ -8,6 +8,7 @@ mod cache; mod geo_alg; mod radio; mod testdata; +mod v2x_tx; fn main() { // env_logger::init();