cam_tx: Fill path history

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"]

feat-permut.sh

@@ -17,7 +17,6 @@ features_more=(
   spat,gnss,screen,cam_tx,cam,denm
 )
 features_test=(
-  spat
   spat,cam,denm
 )
 

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<chrono::Utc>,
     pos_state: super::PositionState,
+
+    /// database for path history, oldest item at the back of the vector
+    path_history: Vec<PathPoint>,
+}
+
+#[derive(Debug, Clone, Copy, PartialEq)]
+struct PathPoint {
+    pos: geo_types::Point,
+    time: chrono::DateTime<chrono::Utc>,
+    /// distance to previous point. Ignore on last point!
+    dist_m: f32,
+}
+
+impl PathPoint {
+    fn new_from_pos(pos: &super::PositionState, time: chrono::DateTime<chrono::Utc>) -> Self {
+        Self {
+            pos: pos.position,
+            time,
+            dist_m: 0.,
+        }
+    }
+
+    fn new_from_pos_with_dist(
+        pos: &super::PositionState,
+        time: chrono::DateTime<chrono::Utc>,
+        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::<chrono::Utc>::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<PathPoint>,
+        time: chrono::DateTime<chrono::Utc>,
+        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<its_container::PathPoint> {
+        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<PathPoint> = 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<PathPoint> = 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);
+    }
+}

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;

src/test.rs

@@ -8,6 +8,7 @@ mod cache;
 mod geo_alg;
 mod radio;
 mod testdata;
+mod v2x_tx;
 
 fn main() {
     // env_logger::init();