tasks: gps: drop complicated usage of Backoff
This commit is contained in:
159
src/tasks/gps.rs
159
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<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
|
||||
#[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();
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user