tdfischer/renderbug-bike
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

tasks: gps: refine the gps sensor state reporting, and start handling GPS time

Browse Source
This commit is contained in:
Victoria Fischer
2026-03-09 10:08:40 +01:00
parent 18458bbf27
commit f650cfa644
1 changed files with 32 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
src/tasks/gps.rs
View File

@@ -6,7 +6,7 @@ use embedded_hal_async::i2c::I2c as _;
use esp_hal::{i2c::master::I2c, Async};
use log::*;
use nalgebra::Vector2;
use nmea::Nmea;
use nmea::{Nmea, sentences::FixType};
use crate::{backoff::Backoff, events::{Measurement, SensorSource, SensorState}};
@@ -17,7 +17,6 @@ pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus:
events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Offline)).await;
Backoff::from_secs(5).forever().attempt(async || {
info!("Initializing GPS");
events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::AcquiringFix)).await;
// Enable a bunch of data? idk
let bytes = "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await?;
@@ -40,41 +39,44 @@ pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus:
let mut parser = Nmea::default();
let mut parsing = false;
let mut has_lock = false;
events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Online)).await;
events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::AcquiringFix)).await;
info!("GPS is ready!");
loop {
let mut buf = [0; 1];
i2c_bus.read(0x10, &mut buf).await.map_err(|_| { Err::<(), ()>(()) }).ok();
if (buf[0] as char == '\n' || buf[0] as char == '\r') && !strbuf.is_empty() {
if let Ok(result) = parser.parse(&strbuf) {
match parser.fix_type {
None if has_lock => {
match parser.parse_for_fix(&strbuf) {
Ok(FixType::Invalid) if has_lock => {
// TODO: Send a Measurement::SensorOffline(SensorSource::GPS) here instead
events.send(Measurement::GPS(None)).await;
events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Degraded)).await;
has_lock = false
},
None => (),
Some(_) => {
Ok(FixType::Invalid) => {
debug!("Waiting for fix {parser:?}");
},
Ok(fix_type) => {
if !has_lock {
has_lock = true;
info!("Got a fix of type {fix_type:?}");
events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Online)).await;
}
// TODO: Send a Measurement::SensorOnline(SensorSource::GPS) here instead
//TODO: 4 satellites seems to be "Some" fix, 6 is a perfect fix
//TODO: Only send updates when we get the correct nmea sentence
if let (Some(lat), Some(lng)) = (parser.latitude, parser.longitude) {
events.send(Measurement::GPS(Some(Vector2::new(lat, lng)))).await;
}
if let (Some(date), Some(time)) = (parser.fix_date, parser.fix_time) {
let now = date.and_time(time).and_utc();
info!("GPS time is {now}");
}
},
Err(nmea::Error::ParsingError(_)) => {
debug!("NMEA could not parse: {strbuf}");
},
Err(err) => {
error!("NMEA error on {strbuf} {err:?}");
}
log::trace!("nmea={result:?} raw={strbuf:?}");
log::trace!("nmea={parser:?}");
log::trace!("speed={:?} altitude={:?} lat={:?} lng={:?} fix={:?}", parser.speed_over_ground, parser.altitude, parser.latitude, parser.longitude, parser.fix_type);
for sat in parser.satellites() {
trace!("\t{} snr={:?} prn={:?}", sat.gnss_type(), sat.snr(), sat.prn())
}
} else {
log::warn!("Unhandled NMEA {strbuf:?}");
}
strbuf = String::new();
parsing = false;