From f803b3c2d4b36747a66aa2d64f098542e4f44e0e Mon Sep 17 00:00:00 2001 From: Victoria Fischer Date: Tue, 24 Mar 2026 12:44:26 +0100 Subject: [PATCH] tasks: gps: drop complicated usage of Backoff --- src/tasks/gps.rs | 159 ++++++++++++++++++++++++----------------------- 1 file changed, 81 insertions(+), 78 deletions(-) diff --git a/src/tasks/gps.rs b/src/tasks/gps.rs index ad90445..abea844 100644 --- a/src/tasks/gps.rs +++ b/src/tasks/gps.rs @@ -1,5 +1,5 @@ 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_time::Timer; use embedded_hal_async::i2c::I2c as _; @@ -8,91 +8,94 @@ use log::*; use nalgebra::Vector2; 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> { + 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 #[embassy_executor::task] 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; - Backoff::from_secs(5).forever().attempt(async || { - 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?; + events.send(Measurement::SensorHardwareStatus(SensorSource::GPS, SensorState::Offline)).await; + if let Err(e) = init_gps(&mut i2c_bus).await { + error!("Failed to initialize GPS: {e:?}"); + return; + } - // 1hz updates - let bytes = "$PMTK220,1000*1F\r\n"; - i2c_bus.write(0x10, bytes.as_bytes()).await?; + let mut strbuf = String::new(); - // 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 - }).await.unwrap(); - - let mut strbuf = String::new(); - - let mut parser = Nmea::default(); - let mut parsing = false; - let mut has_lock = false; - 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() { - 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 - }, - 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:?}"); + let mut parser = Nmea::default(); + let mut parsing = false; + let mut has_lock = false; + 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() { + 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 + }, + 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:?}"); } - 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(); + } }