src: implement ego tracking models, and port shaders from renderbug into here

src/tasks/gps.rs

@@ -0,0 +1,96 @@
+use alloc::string::String;
+use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
+use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::{DynamicSender, Sender}};
+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, Notification}};
+
+#[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: Sender<'static, NoopRawMutex, Measurement, 4>, sink: DynamicSender<'static, Notification>, 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 => {
+                            sink.send(Notification::GPSLost).await;
+                            has_lock = false
+                        },
+                        None => (),
+                        Some(_) => {
+                            if !has_lock {
+                                sink.send(Notification::GPSAcquired).await;
+                                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(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();
+}