diff --git a/src/gps.rs b/src/gps.rs index 28721928f7f2ef03f34d4516ea393ca5dd65dbd3..dd6242f27ef2508a937839540f83dcfd3d56dfc9 100644 --- a/src/gps.rs +++ b/src/gps.rs @@ -1,7 +1,13 @@ 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)