From 33dab98c004327ee6b292c6bdbce5a3cbdbb81bc Mon Sep 17 00:00:00 2001
From: Stephen D <webmaster@scd31.com>
Date: Wed, 17 Jan 2024 17:04:36 -0400
Subject: [PATCH] Make GPS acquire a lock much faster(only lat and lon is
required now). Also stop sending out-of-date coords
---
src/gps.rs | 44 ++++++++++++++++++++++++++++++++++++++++----
1 file changed, 40 insertions(+), 4 deletions(-)
diff --git a/src/gps.rs b/src/gps.rs
index 2872192..dd6242f 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)
--
GitLab