Next iteration with backoffs, tasks, basic skeleton for events

src/tasks/i2c.rs

@@ -0,0 +1,136 @@
+use core::cell::RefCell;
+
+use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, pubsub::DynPublisher};
+use embassy_time::{Delay, Timer};
+use embedded_hal_async::i2c::I2c as _;
+use esp_hal::{i2c::master::I2c, Async};
+use log::{info, warn};
+use mpu6050_dmp::{address::Address, error_async::Error, sensor_async::Mpu6050};
+use nmea::Nmea;
+use alloc::string::String;
+
+use crate::{backoff::Backoff, events::Notification};
+
+#[embassy_executor::task]
+pub async fn mpu_task(events: DynPublisher<'static, Notification>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
+    let backoff = Backoff::from_millis(5);
+    let busref = RefCell::new(Some(bus));
+
+    backoff.forever().attempt::<_, (), ()>(async || {
+        let mut sensor = backoff.attempt(async || {
+            info!("Initializing MPU");
+            match Mpu6050::new(busref.replace(None).unwrap(), Address::default()).await.map_err(|e| { e.i2c }) {
+                Err(i2c) => {
+                    busref.replace(Some(i2c));
+                    Err(())
+                },
+                Ok(mut sensor) => {
+                    match backoff.attempt(async || { mpu_init(&mut sensor).await }).await {
+                        Err(_) => {
+                            busref.replace(Some(sensor.release()));
+                            Err(())
+                        },
+                        Ok(_) => Ok(sensor)
+                    }
+                }
+            }
+        }).await?;
+        info!("MPU is ready!");
+        let sensor_ref = &mut sensor;
+        loop {
+            match backoff.attempt(async || { sensor_ref.motion6().await }).await {
+                Ok((accel_data, gyro_data)) => {
+                    info!("Accel x={} y={} z={}", accel_data.x() as i32, accel_data.y() as i32, accel_data.z() as i32);
+                    info!("Gyro x={} y={} z={}", gyro_data.x() as i32, gyro_data.y() as i32, gyro_data.z() as i32);
+                    Timer::after_millis(5000).await;
+                },
+                Err(e) => {
+                    warn!("Failed to read MPU motion data! {e:?}");
+                    busref.replace(Some(sensor.release()));
+                    return Err(());
+                }
+            };
+        }
+    }).await.unwrap();
+}
+
+async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>) -> Result<(), Error<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>> {
+    let mut delay = Delay;
+    let backoff = Backoff::from_millis(3);
+    info!("Resetting MPU");
+    backoff.attempt(async || {sensor.reset(&mut delay).await}).await?;
+    info!("Configuring sample rate");
+    backoff.attempt(async || {sensor.set_sample_rate_divider(255).await}).await
+}
+
+const GPS_TEST_DATA: &str = include_str!("../test.nmea");
+
+#[embassy_executor::task]
+pub async fn gps_task(events: DynPublisher<'static, Notification>, mut i2c_bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, 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.unwrap();
+        //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) {
+                if parser.fix_type.is_some() != has_lock {
+                    has_lock = parser.fix_type.is_some();
+                    if has_lock {
+                        events.publish(Notification::GPSAcquired).await;
+                    } else {
+                        events.publish(Notification::GPSLost).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(3).await;
+        } else if parsing {
+            strbuf.push(buf[0] as char);
+            Timer::after_millis(3).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;
+        }
+    }
+}

src/tasks/mod.rs

@@ -0,0 +1,2 @@
+pub mod i2c;
+pub mod render;

src/tasks/render.rs

@@ -0,0 +1,68 @@
+use embassy_sync::{blocking_mutex::raw::NoopRawMutex, pubsub::Subscriber};
+use embassy_time::{Duration, Instant, Timer};
+use esp_hal::{rmt::ChannelCreator, Async, peripherals::GPIO5};
+use esp_hal_smartled::{buffer_size_async, SmartLedsAdapterAsync};
+use figments::{hardware::Output, liber8tion::trig::sin8, prelude::Rectangle, render::Sample};
+use log::{info, warn};
+use rgb::Rgb;
+
+use crate::{display::BikeOutput, events::Notification};
+
+
+#[embassy_executor::task]
+pub async fn render(mut events: Subscriber<'static, NoopRawMutex, Notification, 4, 4, 4>, rmt_channel: ChannelCreator<Async, 0>, gpio: GPIO5<'static>) {
+    let rmt_buffer = [0u32; buffer_size_async(178)];
+
+    let target = SmartLedsAdapterAsync::new(rmt_channel, gpio, rmt_buffer);
+
+    // Change this number to use a different number of LEDs
+    //let mut pixbuf = [Default::default(); 16];
+
+    // Change this to adjust the power available; the USB spec says 500ma is the standard limit, but sometimes you can draw more from a power brick
+    const POWER_MA : u32 = 500;
+    
+    // You probably don't need to change these values, unless your LED strip is somehow not 5 volts
+    const POWER_VOLTS : u32 = 5;
+    const MAX_POWER_MW : u32 = POWER_VOLTS * POWER_MA;
+
+    // This value is used as the 'seed' for rendering each frame, allowing us to do things like run the animation backwards, frames for double FPS, or even use system uptime for more human-paced animations
+    let mut frame = 0;
+
+    //let mut target = BrightnessWriter::new(target, MAX_POWER_MW);
+    let mut output = BikeOutput::new(target, MAX_POWER_MW);
+
+    info!("Rendering started!");
+
+    loop {
+        /*if let Some(evt) = events.try_next_message() {
+
+        }*/
+        let start = Instant::now();
+        //pixbuf.blank();
+        output.blank().await.expect("Failed to blank framebuf");
+
+        // Render the frame to the pixbuf, while also calculating the power consumption as we go
+        for (coords, pix) in output.sample(&Rectangle::everything()) {
+            *pix = Rgb::new(sin8(coords.x.wrapping_mul(3).wrapping_add(coords.y.wrapping_mul(3)).wrapping_add(frame)), 0, 0);
+        }
+
+        // Finally, write out the rendered frame
+        //target.write(pixbuf.iter().cloned()).await.expect("Could not write frame");
+        //info!("frame");
+        output.commit().await.expect("Failed to commit frame");
+        //info!("commit");
+
+        let render_duration = Instant::now() - start;
+        let render_budget = Duration::from_millis(16);
+
+        if render_duration < render_budget {
+            let remaining_budget = render_budget - render_duration;
+            Timer::after(remaining_budget).await;
+        } else {
+            warn!("Render stall! Frame took {}ms", render_duration.as_millis());
+        }
+
+        // Increment the frame counter
+        frame += 1;
+    }
+}