applogic: Move to generic position state interface

src/applogic/mod.rs

@@ -47,11 +47,7 @@ pub struct State {
     time: chrono::NaiveDateTime,
 
     #[allow(unused)]
-    position: geo_types::Point,
-    #[allow(unused)]
-    heading_deg: Option<f32>,
-    #[allow(unused)]
-    speed_mps: Option<f32>,
+    pos: PositionState,
 
     #[cfg(feature = "spat")]
     last_prune: chrono::NaiveDateTime,
@@ -122,6 +118,41 @@ pub enum SignalPhase {
     Yellow,
 }
 
+#[allow(unused)]
+#[derive(Debug, Default, Clone, PartialEq)]
+pub struct GnssFix {
+    pub time: Option<chrono::NaiveDateTime>,
+    pub latitude_deg: f64,
+    pub longitude_deg: f64,
+    pub heading_deg: Option<f32>,
+    pub speed_mps: Option<f32>,
+}
+
+#[allow(unused)]
+#[derive(Debug, Default, Clone, PartialEq)]
+pub struct PositionState {
+    pub position: geo_types::Point,
+    pub heading_deg: Option<f32>,
+    pub speed_mps: Option<f32>,
+}
+
+impl From<&GnssFix> for PositionState {
+    fn from(value: &GnssFix) -> Self {
+        let position = geo_types::Point::new(value.longitude_deg, value.latitude_deg);
+
+        Self {
+            position,
+            heading_deg: value.heading_deg,
+            speed_mps: value.speed_mps,
+        }
+    }
+}
+impl From<GnssFix> for PositionState {
+    fn from(value: GnssFix) -> Self {
+        (&value).into()
+    }
+}
+
 #[cfg(feature = "spat")]
 impl core::fmt::Display for SignalPhase {
     fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
@@ -179,9 +210,7 @@ impl State {
         Self {
             initialized: false,
             time: chrono::NaiveDateTime::default(),
-            position: geo_types::Point::default(),
-            heading_deg: None,
-            speed_mps: None,
+            pos: PositionState::default(),
             #[cfg(feature = "spat")]
             last_prune: chrono::NaiveDateTime::default(),
             #[cfg(feature = "spat")]
@@ -191,23 +220,11 @@ impl State {
         }
     }
 
-    #[cfg(all(feature = "esp", feature = "gnss"))]
-    pub fn update_with_gpsfix(&mut self, fix: &embassy_gps::types::GpsFix) {
-        self.position = geo_types::Point::new(fix.longitude_deg, fix.latitude_deg);
-
-        if let Some(heading) = fix.true_course_deg {
-            self.heading_deg = Some(heading);
-        } else {
-            warn!("State: Course missing from GpsFix");
-        }
-
-        if let Some(speed) = fix.speed_over_ground {
-            self.speed_mps = Some(speed);
-        } else {
-            warn!("State: Speed missing from GpsFix");
-        }
-
-        if let Some(time) = fix.get_timestamp() {
+    #[allow(unused)]
+    #[cfg(feature = "esp")]
+    pub fn update_with_gpsfix(&mut self, fix: &GnssFix) {
+        self.pos = fix.into();
+        if let Some(time) = fix.time {
             self.time = time;
 
             if !self.initialized {
@@ -217,18 +234,19 @@ impl State {
         }
     }
 
-    #[cfg(feature = "gnss")]
+    #[allow(unused)]
     pub fn print_fix(&self) -> alloc::string::String {
         alloc::format!(
             "{:?}, {:.4} N, {:.4} E, {:.0}°, {:.2} m/s",
             self.time,
-            self.position.y(),
-            self.position.x(),
-            self.heading_deg.unwrap_or_default(),
-            self.speed_mps.unwrap_or_default(),
+            self.pos.position.y(),
+            self.pos.position.x(),
+            self.pos.heading_deg.unwrap_or_default(),
+            self.pos.speed_mps.unwrap_or_default(),
         )
     }
 
+    #[allow(clippy::unused_self)]
     pub fn prune(&mut self) {
         #[cfg(feature = "spat")]
         if self.last_prune + PRUNE_INTERVAL < self.time {
@@ -252,7 +270,7 @@ impl State {
 
     #[cfg(feature = "spat")]
     pub fn run_glosa(&mut self) -> GlosaOutput {
-        let Some(own_heading) = self.heading_deg else {
+        let Some(own_heading) = self.pos.heading_deg else {
             use alloc::string::ToString;
 
             return GlosaOutput::Error("Heading unknown".to_string());
@@ -268,7 +286,7 @@ impl State {
                 );
 
                 if let Some(search_result) =
-                    Self::find_intersection(&self.position, own_heading, &self.tlc_cache)
+                    Self::find_intersection(&self.pos.position, own_heading, &self.tlc_cache)
                 {
                     info!(
                         "GLOSA: Approach has signal groups: {:?}",
@@ -315,7 +333,7 @@ impl State {
                         &intersection,
                         info.lane_id,
                         info.approach_id,
-                        &self.position,
+                        &self.pos.position,
                     ) {
                         info!("GLOSA: Intersection was left -> back to idle");
                         (

src/main.rs

@@ -40,8 +40,7 @@ const WIFI_CHANNEL: radio::Channel = 180;
 esp_bootloader_esp_idf::esp_app_desc!();
 
 #[cfg(feature = "gnss")]
-static GNSS_UPDATE: Mutex<RefCell<Option<embassy_gps::types::GpsFix>>> =
-    Mutex::new(RefCell::new(None));
+static GNSS_UPDATE: Mutex<RefCell<Option<applogic::GnssFix>>> = Mutex::new(RefCell::new(None));
 static WIFI_RX_QUEUE: Mutex<RefCell<Option<ArrayQueue<Vec<u8>>>>> = Mutex::new(RefCell::new(None));
 
 #[allow(
@@ -341,9 +340,17 @@ async fn gnss_task(
             fix_drop_message = !fix_drop_message;
             if fix_drop_message {
                 // send to main thread
+                let pos_state = applogic::GnssFix {
+                    time: fix.get_timestamp(),
+                    latitude_deg: fix.latitude_deg,
+                    longitude_deg: fix.longitude_deg,
+                    heading_deg: fix.true_course_deg,
+                    speed_mps: fix.speed_over_ground,
+                };
+
                 critical_section::with(|cs| {
                     let gnss_update_ref = GNSS_UPDATE.borrow(cs);
-                    gnss_update_ref.replace(Some(fix));
+                    gnss_update_ref.replace(Some(pos_state));
                 });
             }
         } else {