96 lines
4.3 KiB
Rust
96 lines
4.3 KiB
Rust
use alloc::string::String;
|
|
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
|
|
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::DynamicSender};
|
|
use embassy_time::Timer;
|
|
use embedded_hal_async::i2c::I2c as _;
|
|
use esp_hal::{i2c::master::I2c, Async};
|
|
use log::*;
|
|
use nalgebra::Vector2;
|
|
use nmea::Nmea;
|
|
|
|
use crate::{backoff::Backoff, events::Measurement};
|
|
|
|
#[allow(dead_code)] //FIXME: Allow switching to this via configure option
|
|
const GPS_TEST_DATA: &str = include_str!("../test.nmea");
|
|
|
|
#[embassy_executor::task]
|
|
pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
|
|
Backoff::from_secs(5).forever().attempt::<_, (), ()>(async || {
|
|
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?;
|
|
|
|
// 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
|
|
}).await.unwrap();
|
|
|
|
let mut strbuf = String::new();
|
|
|
|
let mut parser = Nmea::default();
|
|
let mut parsing = false;
|
|
let mut has_lock = false;
|
|
//let mut iter = GPS_TEST_DATA.as_bytes().iter();
|
|
info!("GPS is ready!");
|
|
loop {
|
|
let mut buf = [0; 1];
|
|
i2c_bus.read(0x10, &mut buf).await.map_err(|_| { Err::<(), ()>(()) }).ok();
|
|
//buf[0] = *(iter.next().unwrap());
|
|
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 => {
|
|
events.send(Measurement::GPS(None)).await;
|
|
has_lock = false
|
|
},
|
|
None => (),
|
|
Some(_) => {
|
|
if !has_lock {
|
|
has_lock = true;
|
|
}
|
|
|
|
//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;
|
|
}
|
|
}
|
|
}
|
|
log::info!("nmea={result:?} raw={strbuf:?}");
|
|
log::debug!("nmea={parser:?}");
|
|
log::info!("speed={:?} altitude={:?} lat={:?} lng={:?} fix={:?}", parser.speed_over_ground, parser.altitude, parser.latitude, parser.longitude, parser.fix_type);
|
|
for sat in parser.satellites() {
|
|
info!("\t{} snr={:?} prn={:?}", sat.gnss_type(), sat.snr(), sat.prn())
|
|
}
|
|
} else {
|
|
log::warn!("Unhandled NMEA {strbuf:?}");
|
|
}
|
|
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();
|
|
}
|