Make GPS acquire a lock much faster(only lat and lon is required now). Also...

Make GPS acquire a lock much faster(only lat and lon is required now). Also stop sending out-of-date coords

src/gps.rs

 use nmea::Nmea;
 
+use crate::{app::monotonics::now, MyInstant};
+
 const MAX_SENTENCE_LEN: usize = 128;
 
+// If we don't hear from the GPS in a while,
+// lock was lost and we should remove our coords
+const GPS_EXPIRE_SECS: u64 = 10;
+
 #[derive(Copy, Clone)]
 pub struct GpsPos {
     pub latitude: f64,
@@ -17,6 +23,8 @@ pub struct GpsModule {
 
     sentence: [u8; MAX_SENTENCE_LEN],
     sentence_i: usize,
+
+    last_received: MyInstant,
 }
 
 impl GpsModule {
@@ -27,6 +35,8 @@ impl GpsModule {
 
             sentence: [0; MAX_SENTENCE_LEN],
             sentence_i: 0,
+
+            last_received: now(),
         }
     }
 
@@ -40,6 +50,7 @@ impl GpsModule {
                 if let Ok(sentence) = core::str::from_utf8(&self.sentence[..self.sentence_i]) {
                     if self.nmea.parse(sentence).is_ok() {
                         if let Some(pos) = self.raw_pos() {
+                            self.last_received = now();
                             self.cached_pos = Some(pos);
                         }
                     }
@@ -56,17 +67,42 @@ impl GpsModule {
         }
     }
 
-    pub fn pos(&self) -> Option<GpsPos> {
+    pub fn pos(&mut self) -> Option<GpsPos> {
+        let gps_age_seconds = (now() - self.last_received).to_secs();
+        if gps_age_seconds > GPS_EXPIRE_SECS {
+            return None;
+        }
+
         self.cached_pos
     }
 
     fn raw_pos(&self) -> Option<GpsPos> {
+        // fallback on cached pos. Otherwise use default values
+        let altitude_meters = self
+            .nmea
+            .altitude
+            .or(self.cached_pos.map(|p| p.altitude_meters))
+            .unwrap_or(0.0);
+
+        let speed_ms = self
+            .nmea
+            .speed_over_ground
+            .map(|x| x * 0.514_444_5)
+            .or(self.cached_pos.map(|p| p.speed_ms))
+            .unwrap_or(0.0);
+
+        let true_course = self
+            .nmea
+            .true_course
+            .or(self.cached_pos.map(|p| p.true_course))
+            .unwrap_or(0.0);
+
         let x = GpsPos {
             latitude: self.nmea.latitude?,
             longitude: self.nmea.longitude?,
-            altitude_meters: self.nmea.altitude?,
-            speed_ms: self.nmea.speed_over_ground? * 0.514_444_5,
-            true_course: self.nmea.true_course?,
+            altitude_meters,
+            speed_ms,
+            true_course,
         };
 
         Some(x)