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

tasks: gps: drop complicated usage of Backoff

Browse Source
This commit is contained in:
Victoria Fischer
2026-03-24 12:44:26 +01:00
parent ee5e2e2a4b
commit f803b3c2d4
1 changed files with 81 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

159
src/tasks/gps.rs
View File

@@ -1,5 +1,5 @@
use alloc::string::String; use alloc::string::String;
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice; use embassy_embedded_hal::shared_bus::{I2cDeviceError, asynch::i2c::I2cDevice};
use embassy_sync::{blocking_mutex::raw::NoopRawMutex, channel::DynamicSender}; use embassy_sync::{blocking_mutex::raw::NoopRawMutex, channel::DynamicSender};
use embassy_time::Timer; use embassy_time::Timer;
use embedded_hal_async::i2c::I2c as _; use embedded_hal_async::i2c::I2c as _;
@@ -8,91 +8,94 @@ use log::*;
use nalgebra::Vector2; use nalgebra::Vector2;
use nmea::{Nmea, sentences::FixType}; use nmea::{Nmea, sentences::FixType};
use crate::{backoff::Backoff, events::{Measurement, SensorSource, SensorState}}; use crate::{events::{Measurement, SensorSource, SensorState}};
async fn init_gps(i2c_bus: &mut I2cDevice<'static, NoopRawMutex, I2c<'static, Async>>) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
info!("Initializing GPS");
// 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?;
// 1hz updates
let bytes = "$PMTK220,1000*1F\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await?;
// 1hz position fix
let bytes = "$PMTK300,1000,0,0,0,0*1C\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await?;
// Antenna updates
let bytes = "$PGCMD,33,1*6C\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await
}
// FIXME: We need a way to put the GPS to sleep when the system goes to sleep // FIXME: We need a way to put the GPS to sleep when the system goes to sleep
#[embassy_executor::task] #[embassy_executor::task]
pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus: I2cDevice<'static, NoopRawMutex, I2c<'static, Async>>) { pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus: I2cDevice<'static, NoopRawMutex, I2c<'static, Async>>) {
Backoff::from_secs(5).forever().attempt::<_, (), ()>(async || { events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Offline)).await;
events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Offline)).await; if let Err(e) = init_gps(&mut i2c_bus).await {
Backoff::from_secs(5).forever().attempt(async || { error!("Failed to initialize GPS: {e:?}");
info!("Initializing GPS"); return;
// 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?;
// 1hz updates let mut strbuf = String::new();
let bytes = "$PMTK220,1000*1F\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await?;
// 1hz position fix let mut parser = Nmea::default();
let bytes = "$PMTK300,1000,0,0,0,0*1C\r\n"; let mut parsing = false;
i2c_bus.write(0x10, bytes.as_bytes()).await?; let mut has_lock = false;
events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::AcquiringFix)).await;
// Antenna updates info!("GPS is ready!");
let bytes = "$PGCMD,33,1*6C\r\n"; loop {
i2c_bus.write(0x10, bytes.as_bytes()).await let mut buf = [0; 1];
}).await.unwrap(); 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() {
let mut strbuf = String::new(); match parser.parse_for_fix(&strbuf) {
Ok(FixType::Invalid) if has_lock => {
let mut parser = Nmea::default(); // TODO: Send a Measurement::SensorOffline(SensorSource::GPS) here instead
let mut parsing = false; events.send(Measurement::GPS(None)).await;
let mut has_lock = false; events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Degraded)).await;
events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::AcquiringFix)).await; has_lock = false
info!("GPS is ready!"); },
loop { Ok(FixType::Invalid) => {
let mut buf = [0; 1]; debug!("Waiting for fix {parser:?}");
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() { Ok(fix_type) => {
match parser.parse_for_fix(&strbuf) { if !has_lock {
Ok(FixType::Invalid) if has_lock => { has_lock = true;
// TODO: Send a Measurement::SensorOffline(SensorSource::GPS) here instead info!("Got a fix of type {fix_type:?}");
events.send(Measurement::GPS(None)).await; events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Online)).await;
events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Degraded)).await;
has_lock = false
},
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;
}
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:?}");
} }
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:?}");
} }
strbuf = String::new();
parsing = false;
// Update frequency is 1hz, so we should never get an update faster than once per second
Timer::after_secs(1).await;
} else if strbuf.is_empty() && (buf[0] as char == '$' || buf[0] as char == '!') {
parsing = true;
strbuf.push(buf[0] as char);
Timer::after_millis(10).await;
} else if parsing {
strbuf.push(buf[0] as char);
Timer::after_millis(10).await;
} else {
// If there is no data ready for some reason, wait 500ms, which should place us at least somewhere after the next data frame is ready to read.
Timer::after_millis(500).await;
} }
strbuf = String::new();
parsing = false;
// Update frequency is 1hz, so we should never get an update faster than once per second
Timer::after_secs(1).await;
} else if strbuf.is_empty() && (buf[0] as char == '$' || buf[0] as char == '!') {
parsing = true;
strbuf.push(buf[0] as char);
Timer::after_millis(10).await;
} else if parsing {
strbuf.push(buf[0] as char);
Timer::after_millis(10).await;
} else {
// If there is no data ready for some reason, wait 500ms, which should place us at least somewhere after the next data frame is ready to read.
Timer::after_millis(500).await;
} }
}).await.ok(); }
} }