tasks: usb_power: build++

Cargo.lock

@@ -983,6 +983,16 @@ dependencies = [
  "embedded-hal 1.0.0",
 ]
 
+[[package]]
+name = "embedded-hal-bus"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "513e0b3a8fb7d3013a8ae17a834283f170deaf7d0eeab0a7c1a36ad4dd356d22"
+dependencies = [
+ "critical-section",
+ "embedded-hal 1.0.0",
+]
+
 [[package]]
 name = "embedded-io"
 version = "0.6.1"
@@ -1032,6 +1042,19 @@ dependencies = [
  "embedded-nal",
 ]
 
+[[package]]
+name = "embedded-sdmmc"
+version = "0.9.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ce3c7f9ea039eeafc4a49597b7bd5ae3a1c8e51b2803a381cb0f29ce90fe1ec6"
+dependencies = [
+ "byteorder",
+ "embedded-hal 1.0.0",
+ "embedded-io 0.6.1",
+ "heapless 0.8.0",
+ "log",
+]
+
 [[package]]
 name = "embedded-storage"
 version = "0.3.1"
@@ -2776,7 +2799,11 @@ dependencies = [
  "embassy-sync 0.7.2",
  "embassy-time",
  "embedded-graphics",
+ "embedded-hal 1.0.0",
  "embedded-hal-async",
+ "embedded-hal-bus",
+ "embedded-io 0.6.1",
+ "embedded-sdmmc",
  "embedded-storage",
  "enum-map",
  "enumset",

Cargo.toml

@@ -10,8 +10,7 @@ name = "renderbug-bike"
 path = "./src/bin/main.rs"
 
 [features]
-default = ["real-output", "radio", "motion", "oled"]
-real-output = []
+default = ["radio", "i2c", "mpu6050", "oled"]
 dual-core = []
 simulation = []
 radio = [
@@ -20,11 +19,10 @@ radio = [
   "dep:trouble-host",
   "esp-rtos/esp-radio"
 ]
-motion = ["mpu", "gps"]
+i2c = ["dep:nmea"]
+mpu6050 = ["i2c", "dep:mpu6050-dmp"]
 max-usb-power = []
 wokwi = ["max-usb-power"]
-mpu = ["dep:mpu6050-dmp"]
-gps = ["dep:nmea"]
 oled = ["dep:ssd1306", "dep:display-interface"]
 rtt = ["dep:rtt-target"]
 demo = []
@@ -112,6 +110,10 @@ rmp = { path = "../msgpack-rust/rmp/", default-features = false }
 heapless = { version = "0.9.1", features = ["portable-atomic"] }
 num-traits = { version = "0.2.19", default-features = false }
 rtos-trace = { version = "0.2.1", default-features = false, features = ["trace_impl"] }
+embedded-sdmmc = "0.9.0"
+embedded-hal-bus = "0.3.0"
+embedded-hal = "1.0.0"
+embedded-io = "0.6.1"
 
 [profile.dev]
 # Rust debug is too slow.

board/TODO.md

@@ -8,3 +8,9 @@ R2:
 [ ] some kind of power consumption measuring chip? INA233?
 [ ] protection diodes on usb lines?
 [ ] pull-down resistor before LED strip?
+[ ] CS line on SD card
+
+R3:
+[ ] MP2662 lipoly charger chip on a 3.7v battery at 2500mAh
+[ ] redesign power supply to use a buck booster from the 3.7v battery to 5v for the display, and a simple divider directly off the battery for 3.3v for system power
+[ ] soldering ports to accept 3W at 6VAC to charge the battery from the bike dynamo

board/board.kicad_pcb

@@ -11,7 +11,7 @@
 		(title "Renderbug v4 Motion")
 		(date "2026-03-07")
 		(rev "R1")
-		(comment 4 "AISLER Project ID: EQIHYIOA")
+		(comment 4 "AISLER Project ID: SHZJKPNM")
 	)
 	(layers
 		(0 "F.Cu" signal)

board/board.kicad_sch

@@ -8068,15 +8068,15 @@
 		(fields_autoplaced yes)
 		(uuid "4a82c701-7394-4d9c-b333-aea63771a73c")
 		(property "Reference" "U4"
-			(at 62.23 131.7146 0)
+			(at 64.77 131.7146 0)
 			(effects
 				(font
 					(size 1.27 1.27)
 				)
 			)
 		)
-		(property "Value" "SN74LV1T34DBV"
-			(at 62.23 134.2546 0)
+		(property "Value" "SN74LV1T34DBVRG4"
+			(at 64.77 134.2546 0)
 			(effects
 				(font
 					(size 1.27 1.27)

src/animation.rs

@@ -29,9 +29,9 @@ impl<S: Surface> AnimationActor<Fract8> for S {
 }
 
 #[derive(Debug)]
-pub struct AnimDisplay<'a>(pub &'a mut DisplayControls);
+pub struct AnimDisplay<'a, 'b>(pub &'a mut DisplayControls<'b>);
 
-impl<'a> AnimationActor<Fract8> for AnimDisplay<'a> {
+impl<'a> AnimationActor<Fract8> for AnimDisplay<'a, '_> {
     fn get_value(&self) -> Fract8 {
         self.0.brightness()
     }
@@ -115,7 +115,7 @@ struct Animator<'a, T, const ACTOR_COUNT: usize> {
 impl<'a, T, const ACTOR_COUNT: usize> Animator<'a, T, ACTOR_COUNT> {
 }
 
-impl<T> Animation<T> {
+impl<T: core::fmt::Debug> Animation<T> {
     pub const fn new() -> Self {
         Self {
             from: None,
@@ -128,12 +128,14 @@ impl<T> Animation<T> {
         let mut now: Instant = Instant::now();
         loop {
             // Find the next shortest delay
-            let mut next_keyframe_time = animators[0].next_update;
+            let mut next_keyframe_time = Instant::MAX;
             let mut finished = false;
+            let mut has_valid = false;
             for animator in &mut animators {
                 if !animator.is_valid() {
                     continue;
                 }
+                has_valid = true;
                 if animator.next_update <= now {
                     finished = match animator.tick() {
                         TickResult::Finished => true,
@@ -146,10 +148,13 @@ impl<T> Animation<T> {
                 }
             }
 
+            // If there are no valid animators, or if all animators are finished, then we're done
+            finished |= !has_valid;
             if finished {
                 break;
             }
 
+            assert!(next_keyframe_time > now, "Weird times: {next_keyframe_time:?} is earlier than {now:?} animators={animators:?}");
             let keyframe_delay = next_keyframe_time - now;
             trace!("delay {:?}", keyframe_delay.as_millis());
             Timer::after(keyframe_delay).await;

src/bin/main.rs

@@ -18,11 +18,11 @@ use esp_hal::{
 };
 
 use embassy_sync::{
-    blocking_mutex::raw::NoopRawMutex, channel::DynamicReceiver, once_lock::OnceLock, pubsub::PubSubChannel, signal::Signal
+    blocking_mutex::raw::NoopRawMutex, channel::DynamicReceiver, once_lock::OnceLock, pubsub::PubSubChannel
 };
 use esp_storage::FlashStorage;
 use log::*;
-use renderbug_bike::{events::{Prediction, SensorSource, SensorState}, gpio_interrupt::{InterruptDispatch, PinInterrupt}, graphics::display::DisplayControls, logging::RenderbugLogger, simdata::IMUReading, storage::{SharedFlash, SimDataRecorder}, tasks::{oled::{OledUI, OledUiSurfacePool, oled_ui}, safetyui::{SafetyUi, safety_ui_main}, ui::UiSurfacePool}, tracing::Tracer};
+use renderbug_bike::{events::{Prediction, SensorSource, SensorState}, gpio_interrupt::{InterruptDispatch, PinInterrupt}, graphics::display::DisplayControls, logging::RenderbugLogger, simdata::IMUReading, storage::{SharedFlash, SimDataRecorder, StorageRange}, tasks::{oled::{OledUI, OledUiSurfacePool, oled_ui}, safetyui::{SafetyUi, safety_ui_main}, ui::UiSurfacePool}, tracing::Tracer};
 use renderbug_bike::events::Measurement;
 use static_cell::StaticCell;
 use esp_backtrace as _;
@@ -51,6 +51,10 @@ fn gpio_interrupt_handler() {
     INTERRUPTS.try_get().unwrap().process_interrupts();
 }
 
+static MOTION_BUS: ConstStaticCell<Channel<CriticalSectionRawMutex,Measurement,5> > = ConstStaticCell::new(Channel::new());
+static RECORDING_BUS: ConstStaticCell<PubSubChannel<CriticalSectionRawMutex,Measurement,10, 4, 1> > = ConstStaticCell::new(PubSubChannel::new());
+static PREDICTIONS: ConstStaticCell<PubSubChannel<NoopRawMutex, Prediction, 30, 7, 1>> = ConstStaticCell::new(PubSubChannel::new());
+
 #[esp_rtos::main]
 async fn main(spawner: Spawner) {
     // If we aren't using the second CPU, we can use the bootloader space for the heap instead
@@ -72,18 +76,17 @@ async fn main(spawner: Spawner) {
     esp_rtos::start(sys_timer.alarm0);
     info!("Embassy initialized!");
 
-    static MOTION_BUS: StaticCell<Channel<CriticalSectionRawMutex,Measurement,5> > = StaticCell::new();
-    let motion_bus = MOTION_BUS.init_with(|| { Channel::new() });
-
-    static RECORDING_BUS: StaticCell<PubSubChannel<CriticalSectionRawMutex,Measurement,1, 1, 1> > = StaticCell::new();
-    let recording_bus = RECORDING_BUS.init_with(|| { PubSubChannel::new() });
+    let motion_bus = MOTION_BUS.take();
+    let recording_bus = RECORDING_BUS.take();
 
     info!("Setting up rendering pipeline");
     let mut surfaces = UiSurfacePool::default();
     let ui = Ui::new(&mut surfaces);
 
-    let display_controls = DisplayControls::default();
-    let oled_controls = DisplayControls::default();
+    static MAIN_DISPLAY_SWITCH: ConstStaticCell<DisplayControlResources> = ConstStaticCell::new(DisplayControlResources::new());
+    static OLED_DISPLAY_SWITCH: ConstStaticCell<DisplayControlResources> = ConstStaticCell::new(DisplayControlResources::new());
+    let display_controls = DisplayControls::new(MAIN_DISPLAY_SWITCH.take());
+    let oled_controls = DisplayControls::new(OLED_DISPLAY_SWITCH.take());
 
     let mut oled_surfaces = OledUiSurfacePool::default();
     let oled_uniforms = Default::default();
@@ -98,7 +101,7 @@ async fn main(spawner: Spawner) {
     wdt.enable();
 
     // Spawn the rendering task as soon as possible so it can start pushing pixels
-    spawner.must_spawn(renderbug_bike::tasks::render::render(peripherals.SPI2.degrade(), peripherals.DMA_CH2.degrade(), peripherals.GPIO5.degrade(), surfaces, safety_surfaces, display_controls, wdt));
+    spawner.must_spawn(renderbug_bike::tasks::render::render(peripherals.SPI2.degrade(), peripherals.DMA_CH2.degrade(), peripherals.GPIO5.degrade(), surfaces, safety_surfaces, display_controls.clone(), wdt));
 
     let imu_interrupt = PinInterrupt::new(Input::new(peripherals.GPIO36.degrade(), InputConfig::default()), Event::RisingEdge);
     let pd_interrupt = PinInterrupt::new(Input::new(peripherals.GPIO16.degrade(), InputConfig::default()), Event::RisingEdge);
@@ -114,6 +117,16 @@ async fn main(spawner: Spawner) {
     let mut io = esp_hal::gpio::Io::new(peripherals.IO_MUX);
     io.set_interrupt_handler(gpio_interrupt_handler);
 
+    spawner.must_spawn(renderbug_bike::tasks::sd_card::sdcard_task(
+        peripherals.SPI3.degrade(),
+        peripherals.GPIO41.degrade(),
+        peripherals.GPIO2.degrade(),
+        peripherals.GPIO1.degrade(),
+        Output::new(peripherals.GPIO40.degrade(), esp_hal::gpio::Level::High, OutputConfig::default()),
+        sd_detect_interrupt,
+        recording_bus.dyn_subscriber().unwrap()
+    ));
+
     #[cfg(feature="i2c")]
     {
         use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
@@ -131,7 +144,7 @@ async fn main(spawner: Spawner) {
         spawner.must_spawn(renderbug_bike::tasks::mpu::mpu_task(motion_bus.dyn_sender(), I2cDevice::new(i2c_bus), imu_interrupt));
         
         spawner.must_spawn(renderbug_bike::tasks::gps::gps_task(motion_bus.dyn_sender(), I2cDevice::new(i2c_bus)));
-        spawner.must_spawn(renderbug_bike::tasks::usb_power::usb_task(I2cDevice::new(i2c_bus), pd_interrupt));
+        spawner.must_spawn(renderbug_bike::tasks::usb_power::usb_task(motion_bus.dyn_sender(), I2cDevice::new(i2c_bus), pd_interrupt, display_controls.clone()));
     }
 
     #[cfg(feature="oled")]
@@ -148,32 +161,6 @@ async fn main(spawner: Spawner) {
         spawner.must_spawn(renderbug_bike::tasks::oled_render::oled_render(output, oled_surfaces, oled_uniforms));
     }
 
-    let mut storage = renderbug_bike::storage::SharedFlash::new(esp_storage::FlashStorage::new());
-    let mut partition_buf = [8; 1024];
-    let partitions = esp_bootloader_esp_idf::partitions::read_partition_table(&mut storage, &mut partition_buf).unwrap();
-
-    #[cfg(feature="simulation")]
-    {
-        use renderbug_bike::tasks::simulation::SimDataTable;
-        for sim_data in SimDataTable::open(storage, partitions).expect("Could not find sim data!") {
-            let srcid = sim_data.srcid();
-            info!("Found simulation data for {srcid:?}");
-            if spawner.spawn(renderbug_bike::tasks::simulation::simulation_task(sim_data, motion_bus.dyn_sender())).is_err() {
-                error!("Unable to spawn simulation task for {srcid:?}! Increase the task pool size.");
-            }
-        }
-    }
-
-    #[cfg(not(feature="simulation"))]
-    {
-        use renderbug_bike::storage::SimDataRecorder;
-
-        let recorder = SimDataRecorder::open(storage, partitions).expect("Unable to open sim data partition for writing");
-        //spawner.spawn(record_telemetry(recording_bus.dyn_receiver(), recorder)).unwrap();
-    }
-
-    spawner.spawn(print_sensor_readings(recording_bus.dyn_subscriber().unwrap())).unwrap();
-
     #[cfg(feature="radio")]
     let (wifi, network_device, ble) = {
         info!("Configuring wifi");
@@ -199,9 +186,7 @@ async fn main(spawner: Spawner) {
 
     let core2_main = |spawner: Spawner| {
         info!("Starting application tasks");
-
-        static PREDICTIONS: StaticCell<PubSubChannel<NoopRawMutex, Prediction, 15, 6, 1>> = StaticCell::new();
-        let predictions = PREDICTIONS.init(PubSubChannel::new());
+        let predictions = PREDICTIONS.take();
 
         #[cfg(not(feature="demo"))]
         {
@@ -239,24 +224,72 @@ async fn main(spawner: Spawner) {
             info!("Starting connectivity task");
             spawner.must_spawn(renderbug_bike::tasks::wifi::wifi_connect_task(wifi, motion_bus.dyn_sender()));
 
+            info!("Starting location sampler");
+            static SAMPLER: ConstStaticCell<Watch<NoopRawMutex, Prediction, 1>> = ConstStaticCell::new(Watch::new());
+            let sampler = SAMPLER.take();
+            spawner.must_spawn(renderbug_bike::tasks::wifi::location_sampler(predictions.dyn_subscriber().unwrap(), sampler.dyn_sender()));
+
             info!("Launching HTTP telemetry");
-            spawner.must_spawn(renderbug_bike::tasks::wifi::http_telemetry_task(predictions.dyn_subscriber().unwrap(), stack, motion_bus.dyn_sender()));
+            spawner.must_spawn(renderbug_bike::tasks::wifi::http_telemetry_task(sampler.dyn_receiver().unwrap(), stack, motion_bus.dyn_sender()));
 
             info!("Starting BLE services");
             spawner.must_spawn(renderbug_bike::tasks::ble::ble_task(ble, predictions.dyn_subscriber().unwrap(), spawner));
         }
 
-        #[cfg(feature="dual-core")]
+        spawner.must_spawn(print_sensor_status(predictions.dyn_subscriber().unwrap()));
+
+        let mut storage = SharedFlash::new(esp_storage::FlashStorage::new(peripherals.FLASH).multicore_auto_park());
+        let mut partition_buf = [8; 1024];
+        let partitions = esp_bootloader_esp_idf::partitions::read_partition_table(&mut storage, &mut partition_buf).unwrap();
+
+        #[cfg(any(feature="flash-recording", feature="simulation"))]
         {
-            info!("Launching core 2 watchdog");
-            let timer1 = TimerGroup::new(peripherals.TIMG1);
-            let mut ui_wdt = timer1.wdt;
-            ui_wdt.set_timeout(esp_hal::timer::timg::MwdtStage::Stage0, esp_hal::time::Duration::from_secs(60));
-            ui_wdt.enable();
-            spawner.must_spawn(wdt_task(ui_wdt));
+            use renderbug_bike::tasks::simulation::{SimDataStream, SimDataTable};
+            let sim_partition = SimDataTable::find_partition(storage, partitions).expect("Could not find sim data partition!");
+            info!("Got partition: {sim_partition:?}");
+            let mut sim_table = match SimDataTable::open(sim_partition.clone()) {
+                Ok(table) => table,
+                Err(SimDataError::StreamIndexMissing) => {
+                    info!("Sim data partition not formatted, creating new stream table with {sim_partition:?}");
+                    SimDataTable::create(sim_partition).expect("Unable to create sim data stream table in partition!")
+                },
+                Err(e) => panic!("Error opening sim data stream table: {e:?}")
+            };
+
+            loop {
+                match sim_table.next() {
+                    Some((header, reader)) => {
+                        let srcid = header.id;
+                        info!("Found simulation data for {srcid:?} at {:#x}", reader.abs_start());
+
+                        let stream = SimDataStream::open(reader, srcid);
+
+                        if cfg!(feature="simulation") {
+                            if spawner.spawn(renderbug_bike::tasks::simulation::simulation_task(stream, motion_bus.dyn_sender())).is_err() {
+                                error!("Unable to spawn simulation task for {srcid:?}! Increase the task pool size.");
+                            }
+                        } else if cfg!(feature="flash-recording") && srcid == StreamType::Bundle {
+                            info!("Continuing recording stream");
+                            spawner.spawn(record_telemetry(recording_bus.dyn_subscriber().unwrap(), stream, motion_bus.dyn_sender())).unwrap();
+                            break;
+                        }
+                    },
+                    None if cfg!(feature="flash-recording") => {
+                        // If we already found a recording stream, we break; out of the loop above
+                        let reader = sim_table.append_new_stream(StreamType::Bundle).expect("Unable to create a new recording stream in the sim data partition! Is there enough free space?");
+                        warn!("Starting new recording stream at {:#x}", reader.abs_start());
+                        let recorder = SimDataStream::create(reader, StreamType::Bundle);
+                        spawner.spawn(record_telemetry(recording_bus.dyn_subscriber().unwrap(), recorder, motion_bus.dyn_sender())).unwrap();
+                        break;
+                    },
+                    _ => ()
+                }
+            }
         }
 
-        spawner.must_spawn(print_sensor_status(predictions.dyn_subscriber().unwrap()));
+        spawner.must_spawn(wdt_task(rtc, predictions.dyn_subscriber().unwrap(), oled_controls, display_controls));
+
+        //info!("Final memory stats: {}", esp_alloc::HEAP.stats());
 
         info!("Ready to rock and roll in {}ms", Instant::now().as_millis());
     };
@@ -304,9 +337,17 @@ async fn wdt_task(mut wdt: Wdt<esp_hal::peripherals::TIMG1<'static>>) {
 }
 
 #[embassy_executor::task]
-async fn record_telemetry(firehose: DynamicReceiver<'static, Measurement>, mut storage: SimDataRecorder<SharedFlash<FlashStorage>>) {
+async fn record_telemetry(mut firehose: DynSubscriber<'static, Measurement>, mut storage: SimDataStream<StorageRange<SharedFlash<FlashStorage<'static>>>>, motion: DynamicSender<'static, Measurement>) {
+    let mut skipped_events = 0;
+    while let Ok(Some((_, evt))) = storage.read_next() {
+        trace!("Skipping event {evt:?}");
+        skipped_events += 1;
+    }
+    info!("Skipped {} events to catch up to the end of the recording stream", skipped_events);
+    motion.send(Measurement::SensorHardwareStatus(SensorSource::FlashRecording, SensorState::Online)).await;
+    storage.write_next(AnnotationReading { buf: *b"Telemetry recording started     " }).unwrap();
     loop {
-        match firehose.receive().await {
+        match firehose.next_message_pure().await {
             Measurement::IMU { accel, gyro } => {
                 let reading = IMUReading {
                     accel_x: accel.x as f64,
@@ -316,24 +357,28 @@ async fn record_telemetry(firehose: DynamicReceiver<'static, Measurement>, mut s
                     gyro_y: gyro.y as f64,
                     gyro_z: gyro.z as f64
                 };
-                storage.write_next(reading).unwrap();
-                info!("Wrote IMU to flash");
+                //storage.write_next(reading).unwrap();
+                trace!("Wrote IMU to flash");
             },
-            _ => ()
-        }
-    }
-}
-
-#[embassy_executor::task]
-async fn print_sensor_readings(mut events: DynSubscriber<'static, Measurement>) {
-    loop {
-        match events.next_message_pure().await {
-            Measurement::IMU { accel, gyro } => {
-                esp_println::println!("accel=({},{},{}) gyro=({},{},{})", accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z);
-            },
-            Measurement::GPS(gps) => {
-                esp_println::println!("gps={gps:?}");
+            Measurement::GPS(Some(pos)) => {
+                storage.write_next(GPSReading {
+                    lat: pos.x,
+                    lon: pos.y,
+                }).unwrap();
+                trace!("Wrote GPS to flash");
             },
+            Measurement::SensorHardwareStatus(sensor, status) => {
+                let annotation = alloc::format!("{:?}={:?}", sensor, status);
+                let mut buf = [0; 32];
+                let bytes = annotation.as_bytes();
+                let copy_len = bytes.len().min(buf.len());
+                buf[..copy_len].copy_from_slice(&bytes[..copy_len]);
+                if copy_len < buf.len() {
+                    buf[copy_len..].fill(b' ');
+                }
+                storage.write_next(AnnotationReading { buf }).unwrap();
+                trace!("Wrote sensor status update to flash");
+            }
             _ => ()
         }
     }
@@ -341,8 +386,6 @@ async fn print_sensor_readings(mut events: DynSubscriber<'static, Measurement>)
 
 #[embassy_executor::task]
 async fn print_sensor_status(mut events: DynSubscriber<'static, Prediction>) {
-
-    info!("telemetry ready");
     let mut sensor_states: EnumMap<SensorSource, SensorState> = EnumMap::default();
     loop {
         let next = events.next_message_pure().with_timeout(Duration::from_secs(5)).await;
@@ -352,10 +395,10 @@ async fn print_sensor_status(mut events: DynSubscriber<'static, Prediction>) {
                 let mut report_str = String::new();
                 for (sensor, state) in &sensor_states {
                     let state_icon = match state {
-                        SensorState::AcquiringFix => "?",
-                        SensorState::Degraded => "-",
-                        SensorState::Offline => "X",
-                        SensorState::Online => "O"
+                        SensorState::AcquiringFix => "⏱",
+                        SensorState::Degraded => "!",
+                        SensorState::Offline => "✗",
+                        SensorState::Online => "✔"
                     };
                     report_str += alloc::format!("{sensor:?}={state_icon} ").as_str();
                 }
@@ -365,10 +408,10 @@ async fn print_sensor_status(mut events: DynSubscriber<'static, Prediction>) {
                 let mut report_str = String::new();
                 for (sensor, state) in &sensor_states {
                     let state_icon = match state {
-                        SensorState::AcquiringFix => "?",
-                        SensorState::Degraded => "-",
-                        SensorState::Offline => "X",
-                        SensorState::Online => "O"
+                        SensorState::AcquiringFix => "⏱",
+                        SensorState::Degraded => "!",
+                        SensorState::Offline => "✗",
+                        SensorState::Online => "✔"
                     };
                     report_str += alloc::format!("{sensor:?}={state_icon} ").as_str();
                 }

src/bin/playback.rs

@@ -0,0 +1,59 @@
+#![no_std]
+#![no_main]
+
+use embassy_executor::Spawner;
+use esp_hal::clock::CpuClock;
+use esp_hal::timer::systimer::SystemTimer;
+use log::*;
+use esp_backtrace as _;
+
+use renderbug_bike::logging::RenderbugLogger;
+use renderbug_bike::tasks::simulation::{SimDataStream, SimDataTable};
+use renderbug_bike::simdata::*;
+
+esp_bootloader_esp_idf::esp_app_desc!();
+
+#[esp_rtos::main]
+async fn main(spawner: Spawner) {
+
+    esp_alloc::heap_allocator!(size: 100000);
+
+    RenderbugLogger::init_logger();
+
+    let config = esp_hal::Config::default().with_cpu_clock(CpuClock::max());
+    let peripherals = esp_hal::init(config);
+    let sys_timer = SystemTimer::new(peripherals.SYSTIMER);
+    esp_rtos::start(sys_timer.alarm0);
+
+    let mut storage = renderbug_bike::storage::SharedFlash::new(esp_storage::FlashStorage::new(peripherals.FLASH).multicore_auto_park());
+    let mut partition_buf = [8; 1024];
+    let partitions = esp_bootloader_esp_idf::partitions::read_partition_table(&mut storage, &mut partition_buf).unwrap();
+
+    let sim_partition = SimDataTable::find_partition(storage, partitions).expect("Could not find sim data partition!");
+    info!("Got partition: {sim_partition:?}");
+    let sim_table = SimDataTable::open(sim_partition).expect("Sim data partition not found");
+
+    for (header, reader) in sim_table {
+        info!("Found stream {header:?}");
+        if header.id == StreamType::Bundle {
+            let mut stream = SimDataStream::open(reader, header.id);
+            let mut timestamp = 0;
+            loop {
+                match stream.read_next() {
+                    Ok(Some((timecode, next_evt))) => {
+                        timestamp += timecode.as_millis();
+                        esp_println::println!("{timestamp} {next_evt:?}");
+                    },
+                    Ok(None) => {
+                        warn!("End of simulation data stream");
+                        break
+                    },
+                    Err(err) => {
+                        warn!("Error during sensor stream: {err:?}");
+                        break
+                    }
+                }
+            }
+        }
+    }
+}

src/bin/reset.rs

@@ -0,0 +1,36 @@
+#![no_std]
+#![no_main]
+
+use embassy_executor::Spawner;
+use esp_hal::clock::CpuClock;
+use esp_hal::timer::systimer::SystemTimer;
+use log::*;
+use esp_backtrace as _;
+
+use renderbug_bike::logging::RenderbugLogger;
+use renderbug_bike::tasks::simulation::{SimDataStream, SimDataTable};
+use renderbug_bike::simdata::*;
+
+esp_bootloader_esp_idf::esp_app_desc!();
+
+#[esp_rtos::main]
+async fn main(spawner: Spawner) {
+
+    esp_alloc::heap_allocator!(size: 100000);
+
+    RenderbugLogger::init_logger();
+
+    let config = esp_hal::Config::default().with_cpu_clock(CpuClock::max());
+    let peripherals = esp_hal::init(config);
+    let sys_timer = SystemTimer::new(peripherals.SYSTIMER);
+    esp_rtos::start(sys_timer.alarm0);
+
+    let mut storage = renderbug_bike::storage::SharedFlash::new(esp_storage::FlashStorage::new(peripherals.FLASH));
+    let mut partition_buf = [8; 1024];
+    let partitions = esp_bootloader_esp_idf::partitions::read_partition_table(&mut storage, &mut partition_buf).unwrap();
+
+    let sim_partition = SimDataTable::find_partition(storage, partitions).expect("Could not find sim data partition!");
+    info!("Got partition: {sim_partition:?}");
+    SimDataTable::create(sim_partition).expect("Could not write empty sim partition");
+    error!("Overwrote sim data partition with a blank stream table");
+}

src/ego/engine.rs

@@ -1,5 +1,5 @@
 use embassy_sync::pubsub::DynPublisher;
-use embassy_time::{Duration, Instant};
+use embassy_time::{Duration, Instant, WithTimeout};
 use nalgebra::{Rotation3, Vector2, Vector3, ComplexField, RealField};
 use log::*;
 
@@ -60,6 +60,14 @@ const ACCELERATION_NOISE_GATE: f32 = 1.0;
 /// When we get a heading via GPS, this determines how much weight that value has when blended into the system.
 const GPS_HEADING_ALPHA_CORRECTION: f32 = 0.9;
 
+const TIMEOUT: Duration = Duration::from_millis(3);
+
+macro_rules! publish {
+    ($predictions:expr, $prediction:expr) => {
+        $predictions.publish($prediction).with_timeout(TIMEOUT).await.expect("Could not commit IMU data in time. Is the prediction bus stalled?")
+    }
+}
+
 impl Debug for BikeStates {
     fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
         f.debug_struct("BikeStates")
@@ -132,18 +140,18 @@ impl BikeStates {
         let last_motion = self.motion_state.value;
 
         if let Some(true) = self.acquiring_data.read_tripped() {
-            predictions.publish(Prediction::SensorStatus(SensorSource::MotionFrame, SensorState::AcquiringFix)).await;
-            predictions.publish(Prediction::SensorStatus(SensorSource::Location, SensorState::AcquiringFix)).await;
+            publish!(predictions, Prediction::SensorStatus(SensorSource::MotionFrame, SensorState::AcquiringFix));
+            publish!(predictions, Prediction::SensorStatus(SensorSource::Location, SensorState::AcquiringFix));
         }
 
         if let Some(true) = self.has_motion_frame.read_tripped() {
-            predictions.publish(Prediction::SensorStatus(SensorSource::MotionFrame, SensorState::Online)).await
+            publish!(predictions, Prediction::SensorStatus(SensorSource::MotionFrame, SensorState::Online));
         }
 
         match self.has_gps_fix.read_tripped() {
             None => (),
-            Some(true) => predictions.publish(Prediction::SensorStatus(SensorSource::Location, SensorState::Online)).await,
-            Some(false) => predictions.publish(Prediction::SensorStatus(SensorSource::Location, SensorState::Degraded)).await,
+            Some(true) => publish!(predictions, Prediction::SensorStatus(SensorSource::Location, SensorState::Online)),
+            Some(false) => publish!(predictions, Prediction::SensorStatus(SensorSource::Location, SensorState::Degraded)),
         }
 
         let est = self.kf.x;
@@ -155,7 +163,7 @@ impl BikeStates {
         if let Some(pos) = position {
             self.predicted_location.set(pos);
             if let Some(pos) = self.predicted_location.read_tripped() {
-                predictions.publish(Prediction::Location(pos)).await;
+                publish!(predictions, Prediction::Location(pos));
             }
         }
 
@@ -172,24 +180,24 @@ impl BikeStates {
                 }).sum::<f32>();
                 // Also grab the average velocity of the last few sample periods
                 let average_speed = self.speedo.mean();
-                info!("prediction={current_prediction:?} mean={average_speed}");
+                trace!("prediction={current_prediction:?} mean={average_speed}");
 
                 // Reported velocity is kept only to the first decimal, so we aren't spamming the system with floating point noise
                 self.reported_velocity.set((average_speed * 10.0).trunc() / 10.0);
                 if let Some(reported) = self.reported_velocity.read_tripped() {
-                    predictions.publish(Prediction::Velocity(reported)).await;
+                    publish!(predictions, Prediction::Velocity(reported));
                 }
 
                 // We only want to wake up from sleep if our current velocity is something like a bump
                 if average_speed > BUMP_SPEED_NOISE_GATE && self.sleep_timer.wake() {
                     warn!("Waking from sleep into idle mode");
-                    predictions.publish(Prediction::SetPersonality(Personality::Waking)).await;
-                    predictions.publish(Prediction::SetPersonality(Personality::Parked)).await;
+                    publish!(predictions, Prediction::SetPersonality(Personality::Waking));
+                    publish!(predictions, Prediction::SetPersonality(Personality::Parked));
                 }
                 // Here, we additionally release the parking brake if we are currently parked and reading substantial movement
                 if average_speed > WAKEUP_SPEED_NOISE_GATE && self.parking_timer.wake() {
                     warn!("Disengaging parking brake");
-                    predictions.publish(Prediction::SetPersonality(Personality::Active)).await;
+                    publish!(predictions, Prediction::SetPersonality(Personality::Active));
                 }
 
                 // If the total slope is more upwards than not, we are accelerating.
@@ -213,19 +221,19 @@ impl BikeStates {
             //debug!("state={state:?} trend={trend} mean={mean} v={v}");
             //if state != MotionState::Stationary {
             //    warn!("Active due to motion");
-            //    predictions.publish(Prediction::SetPersonality(Personality::Active)).await;
+            //    publish!(predictions, Prediction::SetPersonality(Personality::Active));
             //}
-            predictions.publish(Prediction::Motion { prev: last_motion, next: state }).await;
+            publish!(predictions, Prediction::Motion { prev: last_motion, next: state });
         } else if self.motion_state.value == MotionState::Stationary {
             // Finally, if we are stationary, check our parking and sleep timers
             if self.parking_timer.check() {
                 warn!("Engaging parking brake");
-                predictions.publish(Prediction::SetPersonality(Personality::Parked)).await;
+                publish!(predictions, Prediction::SetPersonality(Personality::Parked));
             }
 
             if self.sleep_timer.check() {
                 warn!("Sleeping!");
-                predictions.publish(Prediction::SetPersonality(Personality::Sleeping)).await;
+                publish!(predictions, Prediction::SetPersonality(Personality::Sleeping));
             }
         }
     }

src/events.rs

@@ -3,6 +3,7 @@ use embassy_time::Duration;
 use enum_map::Enum;
 use enumset::EnumSetType;
 use figments::liber8tion::interpolate::Fract8;
+use figments_render::power::Milliwatts;
 use nalgebra::{Vector2, Vector3};
 
 use crate::ego::engine::MotionState;
@@ -38,13 +39,15 @@ pub enum Measurement {
     GPS(Option<Vector2<f64>>),
     // Accelerometer values in body frame where x=forwards
     IMU { accel: Vector3<f32>, gyro: Vector3<f32> },
+    // Power status
+    ExternalPower(Milliwatts),
 
     // Hardware status updates
     SensorHardwareStatus(SensorSource, SensorState),
 
     // Simulation metadata updates
     SimulationProgress(SensorSource, Duration, Fract8),
-    Annotation
+    Annotation([u8; 32])
 }
 
 #[derive(Default, Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)]
@@ -73,10 +76,14 @@ pub enum SensorSource {
     // Real hardware
     IMU,
     GPS,
+    ExternalPower,
 
     // Connectivity related
     Wifi,
 
+    // Data processing/logging
+    FlashRecording,
+
     // Fusion outputs
     MotionFrame,
     Location,

src/graphics/display.rs

@@ -1,10 +1,11 @@
 use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal, watch::{Receiver, Watch}};
 use figments::{liber8tion::interpolate::Fract8, prelude::*};
-use core::{fmt::Debug, ops::Mul, sync::atomic::{AtomicBool, AtomicU8}};
+use portable_atomic::AtomicU32;
+use core::{cell::RefCell, fmt::Debug, ops::Mul, sync::atomic::{AtomicBool, AtomicU8}};
 use alloc::sync::Arc;
 
 use figments_render::{
-    gamma::{GammaCurve, WithGamma}, output::{Brightness, GammaCorrected, Output, OutputAsync}, power::AsMilliwatts, smart_leds::PowerManagedWriter
+    gamma::{GammaCurve, WithGamma}, output::{Brightness, GammaCorrected, Output, OutputAsync}, power::{AsMilliwatts, Milliwatts}, smart_leds::PowerManagedWriter
 };
 use smart_leds::{SmartLedsWrite, SmartLedsWriteAsync};
 
@@ -13,7 +14,7 @@ pub const NUM_PIXELS: usize = 178;
 pub struct BikeOutput<T, Color> {
     pixbuf: [Color; NUM_PIXELS],
     writer: PowerManagedWriter<T>,
-    controls: DisplayControls
+    controls: DisplayControls<'static>
 }
 
 impl<T, Color> GammaCorrected for BikeOutput<T, Color> {
@@ -23,10 +24,10 @@ impl<T, Color> GammaCorrected for BikeOutput<T, Color> {
 }
 
 impl<T, Color: Default + Copy> BikeOutput<T, Color> {
-    pub fn new(target: T, max_mw: u32, controls: DisplayControls) -> Self {
+    pub fn new(target: T, controls: DisplayControls<'static>) -> Self {
         Self {
             pixbuf: [Default::default(); NUM_PIXELS],
-            writer: PowerManagedWriter::new(target, max_mw),
+            writer: PowerManagedWriter::new(target, controls.max_power()),
             controls
         }
     }
@@ -39,11 +40,12 @@ impl<T, Color: Default + Copy> BikeOutput<T, Color> {
 impl<'a, T: SmartLedsWrite + 'a> Output<'a, SegmentSpace> for BikeOutput<T, T::Color> where T::Color: AsMilliwatts + Copy + Mul<Fract8, Output = T::Color> + WithGamma + Default + Debug + 'a + 'static {
     type Error = T::Error;
 
-    type Controls = DisplayControls;
+    type Controls = DisplayControls<'static>;
 
     fn commit(&mut self)  -> Result<(), Self::Error> {
         self.writer.controls().set_brightness(self.controls.brightness());
         self.writer.controls().set_on(self.controls.is_on());
+        self.writer.controls().set_max_power(self.controls.max_power());
         critical_section::with(|_| {
             self.writer.write(&self.pixbuf)
         })
@@ -58,12 +60,13 @@ impl<'a, T: SmartLedsWriteAsync + 'a> OutputAsync<'a, SegmentSpace> for BikeOutp
     async fn commit_async(&mut self) -> Result<(), T::Error> where T: SmartLedsWriteAsync {
         self.writer.controls().set_brightness(self.controls.brightness());
         self.writer.controls().set_on(self.controls.is_on());
+        self.writer.controls().set_max_power(self.controls.max_power());
         // TODO: We should grab the power used here and somehow feed it back into the telemetry layer, probably just via another atomic u32
         self.writer.write_async(&self.pixbuf).await
     }
     
     type Error = T::Error;
-    type Controls = DisplayControls;
+    type Controls = DisplayControls<'static>;
     
     fn controls(&mut self) -> Option<&mut Self::Controls> {
         Some(&mut self.controls)
@@ -165,7 +168,8 @@ pub const LOW_POWER_FPS: u8 = 16;
 struct ControlData {
     on: AtomicBool,
     brightness: AtomicU8,
-    fps: AtomicU8
+    fps: AtomicU8,
+    max_power_mw: AtomicU32
 }
 
 impl Default for ControlData {
@@ -173,32 +177,63 @@ impl Default for ControlData {
         Self {
             on: AtomicBool::new(true),
             brightness: AtomicU8::new(255),
-            fps: AtomicU8::new(DEFAULT_FPS)
+            fps: AtomicU8::new(DEFAULT_FPS),
+            // We default to 500mA, because that's what we get over USB by default
+            max_power_mw: AtomicU32::new(500)
         }
     }
 }
 
-// A watch that indicates whether or not the rendering engine is running. If the display is off or sleeping, this will be false.
-static RENDER_IS_RUNNING: Watch<CriticalSectionRawMutex, bool, 7> = Watch::new();
-
-// TODO: Implement something similar for a system-wide sleep mechanism
-pub struct DisplayControls {
-    data: Arc<ControlData>,
-    display_is_on: Arc<Signal<CriticalSectionRawMutex, bool>>,
-    render_run_receiver: Receiver<'static, CriticalSectionRawMutex, bool, 7>
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum RenderState {
+    On,
+    Off
 }
 
-impl Clone for DisplayControls {
+pub struct DisplayControlResources {
+    state_rx: Watch<CriticalSectionRawMutex, RenderState, 7>,
+    state_ack: Watch<CriticalSectionRawMutex, RenderState, 7>
+}
+
+impl DisplayControlResources {
+    pub const fn new() -> Self {
+        Self {
+            state_rx: Watch::new_with(RenderState::On),
+            state_ack: Watch::new_with(RenderState::On)
+        }
+     }
+}
+
+// TODO: Implement something similar for a system-wide sleep mechanism
+pub struct DisplayControls<'a> {
+    data: Arc<ControlData>,
+    render_run_receiver: Receiver<'a, CriticalSectionRawMutex, RenderState, 7>,
+    render_ack_receiver: Receiver<'a, CriticalSectionRawMutex, RenderState, 7>,
+    resources: &'a DisplayControlResources,
+}
+
+impl Clone for DisplayControls<'_> {
     fn clone(&self) -> Self {
         Self {
             data: Arc::clone(&self.data),
-            display_is_on: Arc::clone(&self.display_is_on),
-            render_run_receiver: RENDER_IS_RUNNING.receiver().expect("Could not create enough render running receivers")
+            render_run_receiver: self.resources.state_rx.receiver().expect("Could not create enough render running receivers"),
+            resources: self.resources,
+            render_ack_receiver: self.resources.state_ack.receiver().expect("Could not create enough render ack receivers"),
         }
     }
 }
 
-impl DisplayControls {
+
+impl<'a> DisplayControls<'a> {
+    pub fn new(resources: &'a DisplayControlResources) -> Self {
+        Self {
+            data: Default::default(),
+            render_run_receiver: resources.state_rx.receiver().expect("Could not create enough render running receivers"),
+            render_ack_receiver: resources.state_ack.receiver().expect("Could not create enough render ack receivers"),
+            resources
+        }
+    }
+
     pub fn is_on(&self) -> bool {
         self.data.on.load(core::sync::atomic::Ordering::Relaxed)
     }
@@ -215,57 +250,55 @@ impl DisplayControls {
         self.data.fps.store(value, core::sync::atomic::Ordering::Relaxed);
     }
 
-    pub async fn wait_until_display_is_turned_on(&self) {
-        while !self.display_is_on.wait().await { log::info!("wait for display") }
-        log::trace!("display says on!");
+    pub fn set_max_power(&mut self, mw: Milliwatts) {
+        self.data.max_power_mw.store(mw.0, core::sync::atomic::Ordering::Relaxed);
     }
 
-    pub fn notify_render_is_running(&mut self, value: bool) {
-        log::trace!("render is running!");
-        RENDER_IS_RUNNING.sender().send(value);
+    pub fn max_power(&self) -> Milliwatts {
+        Milliwatts(self.data.max_power_mw.load(core::sync::atomic::Ordering::Relaxed))
     }
 
-    pub async fn wait_until_render_is_running(&mut self) {
-        while !self.render_run_receiver.changed().await { log::info!("wait for render run") }
-        log::trace!("render says run!");
+    pub async fn wait_for_state(&mut self, state: RenderState) {
+        self.render_run_receiver.get_and(|cur| { *cur == state }).await;
+    }
+
+    pub async fn wait_for_ack(&mut self, state: RenderState) {
+        self.render_ack_receiver.get_and(|cur| { *cur == state }).await;
+    }
+
+    /// Indicates the current state has been applied to the hardware, which can wake up tasks waiting for the power to turn on/off.
+    pub async fn ack(&mut self) {
+        let next_state = self.render_run_receiver.get().await;
+        let is_on = next_state == RenderState::On;
+        self.data.on.store(is_on, core::sync::atomic::Ordering::Relaxed);
+        self.resources.state_ack.sender().send(if self.is_on() { RenderState::On } else { RenderState::Off });
     }
 }
 
-impl GammaCorrected for DisplayControls {
+impl GammaCorrected for DisplayControls<'_> {
     fn set_gamma(&mut self, _gamma: GammaCurve) {
         todo!()
     }
 }
 
-impl Brightness for DisplayControls {
+impl Brightness for DisplayControls<'_> {
     fn set_brightness(&mut self, brightness: Fract8) {
         self.data.brightness.store(brightness.to_raw(), core::sync::atomic::Ordering::Relaxed);
     }
 
     fn set_on(&mut self, is_on: bool) {
-        self.data.on.store(is_on, core::sync::atomic::Ordering::Relaxed);
+        self.resources.state_rx.sender().send(if is_on { RenderState::On } else { RenderState::Off });
         log::trace!("display is on {is_on}");
-        self.display_is_on.signal(is_on);
     }
 }
 
-impl core::fmt::Debug for DisplayControls {
+impl core::fmt::Debug for DisplayControls<'_> {
     fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> Result<(), core::fmt::Error> {
         f.debug_struct("DisplayControls")
             .field("on", &self.data.on)
             .field("brightness", &self.data.brightness)
             .field("fps", &self.data.fps)
-            .field("render_pause_signaled", &self.display_is_on.signaled())
+            .field("max_power_mw", &self.data.max_power_mw)
             .finish()
     }
 }
-
-impl Default for DisplayControls {
-    fn default() -> Self {
-        Self {
-            data: Default::default(),
-            display_is_on: Default::default(),
-            render_run_receiver: RENDER_IS_RUNNING.receiver().unwrap()
-        }
-    }
-}

src/graphics/mod.rs

@@ -4,6 +4,8 @@ pub mod shaders;
 pub mod oled_ui;
 
 mod images {
+    #![allow(unused)]
+
     use embedded_graphics::{
         image::ImageRaw,
         pixelcolor::BinaryColor

src/graphics/ssd1306.rs

@@ -106,7 +106,7 @@ impl<'a> Iterator for SsdSampler<'a> {
 pub struct SsdOutput {
     pixbuf: [u8; 128 * 64 / 8], 
     target: ssd1306::Ssd1306Async<ssd1306::prelude::I2CInterface<esp_hal::i2c::master::I2c<'static, esp_hal::Async>>, ssd1306::prelude::DisplaySize128x64, ssd1306::mode::BasicMode>,
-    controls: DisplayControls,
+    controls: DisplayControls<'static>,
     is_on: bool,
     last_brightness: Fract8,
     reset_pin: Output<'static>
@@ -124,7 +124,7 @@ impl SsdOutput {
         Some(SsdPixel { byte: pixref, bit, coords })
     }
 
-    pub async fn new(i2c: I2c<'static, Async>, reset_pin: Output<'static>, controls: DisplayControls) -> Self {
+    pub async fn new(i2c: I2c<'static, Async>, reset_pin: Output<'static>, controls: DisplayControls<'static>) -> Self {
         let interface = I2CDisplayInterface::new(i2c);
         let target = Ssd1306Async::new(interface, DisplaySize128x64, DisplayRotation::Rotate0);
 
@@ -221,7 +221,7 @@ impl OriginDimensions for SsdOutput {
 impl<'a> OutputAsync<'a, Matrix2DSpace> for SsdOutput {
     type Error = DisplayError;
 
-    type Controls = DisplayControls;
+    type Controls = DisplayControls<'static>;
 
     async fn commit_async(&mut self)  -> Result<(), Self::Error> {
         let new_brightness = self.controls.brightness();

src/logging.rs

@@ -22,7 +22,6 @@ impl Default for RenderbugLogger {
     }
 }
 
-//static LOGGER: StaticCell<RenderbugLogger> = StaticCell::new();
 static mut LOGGER: Option<RenderbugLogger> = None;
 
 impl RenderbugLogger {

src/simdata.rs

@@ -1,7 +1,7 @@
-use rmp::{Marker, decode::{ExtMeta, RmpRead, ValueReadError}, encode::{RmpWrite, ValueWriteError}};
+use rmp::{decode::{ExtMeta, RmpRead, RmpReadErr, ValueReadError}, encode::{RmpWrite, RmpWriteErr, ValueWriteError}};
 
 pub trait RmpData: Sized {
-    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>>;
+    fn from_rmp<Reader: RmpRead<Error = E>, E: RmpReadErr + RmpWriteErr>(reader: &mut Reader) -> Result<Self, SimDataError<E>>;
     fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>>;
 }
 
@@ -11,24 +11,32 @@ pub trait EventRecord: RmpData {
 }
 
 #[derive(Debug)]
-pub enum SimDataError<E> {
+pub enum SimDataError<E: RmpReadErr + RmpWriteErr> {
     StreamIndexMissing,
     InvalidChunkSize { expected: usize, found: usize },
     MissingTimecode,
     BadString,
-    DecodeError(E),
+    DecodeError(ValueReadError<E>),
+    EncodeError(ValueWriteError<E>),
     EndOfStream,
     UnsupportedStreamType(ExtMeta),
     EventHeaderMissing,
-    PartitionNotFound
+    PartitionNotFound,
+    NotEnoughSpace
 }
 
-impl<E> From<E> for SimDataError<E> {
-    fn from(value: E) -> Self {
+impl<E: RmpReadErr + RmpWriteErr> From<ValueReadError<E>> for SimDataError<E> {
+    fn from(value: ValueReadError<E>) -> Self {
         SimDataError::DecodeError(value)
     }
 }
 
+impl<E: RmpReadErr + RmpWriteErr> From<ValueWriteError<E>> for SimDataError<E> {
+    fn from(value: ValueWriteError<E>) -> Self {
+        SimDataError::EncodeError(value)
+    }
+}
+
 #[derive(Debug, Clone, Copy, PartialEq, Eq)]
 pub enum StreamType {
     IMU,
@@ -68,11 +76,11 @@ pub struct StreamIndex {
 }
 
 impl RmpData for StreamIndex {
-    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+    fn from_rmp<Reader: RmpRead<Error = E>, E: RmpReadErr + RmpWriteErr>(reader: &mut Reader) -> Result<Self, SimDataError<E>> {
         if rmp::decode::read_u16(reader)? != 0xDA1A {
             Err(SimDataError::StreamIndexMissing)
         } else {
-            rmp::decode::read_array_len(reader).map(|count| {
+            rmp::decode::read_u64(reader).map(|count| {
                 Self {
                     count: count as usize
                 }
@@ -82,7 +90,7 @@ impl RmpData for StreamIndex {
 
     fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
         rmp::encode::write_u16(writer, 0xDA1A)?;
-        rmp::encode::write_array_len(writer, self.count as u32)?;
+        rmp::encode::write_u64(writer, self.count as u64)?;
         Ok(())
     }
 }
@@ -95,7 +103,7 @@ pub struct BundleEventHeader {
 }
 
 impl RmpData for BundleEventHeader {
-    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+    fn from_rmp<Reader: RmpRead<Error = E>, E: RmpReadErr + RmpWriteErr>(reader: &mut Reader) -> Result<Self, SimDataError<E>> {
         let meta = rmp::decode::read_i8(reader)?;
         if let Ok(id) = meta.try_into() {
             Ok(Self {
@@ -118,7 +126,7 @@ pub struct StreamHeader {
 }
 
 impl RmpData for StreamHeader {
-    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+    fn from_rmp<Reader: RmpRead<Error = E>, E: RmpReadErr + RmpWriteErr>(reader: &mut Reader) -> Result<Self, SimDataError<E>> {
         let meta = rmp::decode::read_ext_meta(reader)?;
         if let Ok(id) = meta.typeid.try_into() {
             Ok(Self {
@@ -142,14 +150,14 @@ pub struct EventStreamHeader {
 }
 
 impl RmpData for EventStreamHeader {
-    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+    fn from_rmp<Reader: RmpRead<Error = E>, E: RmpReadErr + RmpWriteErr>(reader: &mut Reader) -> Result<Self, SimDataError<E>> {
         Ok(Self {
-            count: rmp::decode::read_array_len(reader)? as usize
+            count: rmp::decode::read_u32(reader)? as usize
         })
     }
 
     fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
-        rmp::encode::write_array_len(writer, self.count as u32)?;
+        rmp::encode::write_u32(writer, self.count as u32)?;
         Ok(())
     }
 }
@@ -161,7 +169,7 @@ pub struct StreamEvent<Event: EventRecord> {
 }
 
 impl<Event: EventRecord> RmpData for StreamEvent<Event> {
-    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+    fn from_rmp<Reader: RmpRead<Error = E>, E: RmpReadErr + RmpWriteErr>(reader: &mut Reader) -> Result<Self, SimDataError<E>> {
         let chunk_len = rmp::decode::read_array_len(reader).map_err(|_| { SimDataError::EventHeaderMissing })? as usize;
         // Add 1 to the field count for the timestamp
         if chunk_len != Event::field_count() + 1 {
@@ -209,7 +217,7 @@ impl EventRecord for IMUReading {
 }
 
 impl RmpData for GPSReading {
-    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+    fn from_rmp<Reader: RmpRead<Error = E>, E: RmpReadErr + RmpWriteErr>(reader: &mut Reader) -> Result<Self, SimDataError<E>> {
         Ok(Self {
             lat: rmp::decode::read_f64(reader)?,
             lon: rmp::decode::read_f64(reader)?
@@ -233,7 +241,7 @@ pub struct IMUReading {
 }
 
 impl RmpData for IMUReading {
-    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+    fn from_rmp<Reader: RmpRead<Error = E>, E: RmpReadErr + RmpWriteErr>(reader: &mut Reader) -> Result<Self, SimDataError<E>> {
         Ok(Self {
             accel_x: rmp::decode::read_f64(reader)?,
             accel_y: rmp::decode::read_f64(reader)?,
@@ -263,7 +271,7 @@ pub struct AnnotationReading {
 }
 
 impl RmpData for AnnotationReading {
-    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+    fn from_rmp<Reader: RmpRead<Error = E>, E: RmpReadErr + RmpWriteErr>(reader: &mut Reader) -> Result<Self, SimDataError<E>> {
         let mut buf = [0; 32];
         rmp::decode::read_str(reader, &mut buf).map_err(|_| { SimDataError::BadString })?;
         Ok(Self {

src/storage.rs

@@ -1,13 +1,14 @@
 use core::{cell::RefCell, fmt::Formatter};
 
 use alloc::rc::Rc;
+use embedded_io::{ErrorKind, ErrorType, Read, Write};
 use embedded_storage::{ReadStorage, Storage};
 use esp_bootloader_esp_idf::partitions::PartitionTable;
 use esp_hal::time::Instant;
 use log::*;
 use rmp::{decode::{RmpRead, RmpReadErr}, encode::{RmpWrite, RmpWriteErr, ValueWriteError}};
 
-use crate::simdata::{BundleEventHeader, EventRecord, RmpData, SimDataError, StreamEvent, StreamHeader, StreamIndex, StreamType};
+use crate::{simdata::{BundleEventHeader, EventRecord, EventStreamHeader, RmpData, SimDataError, StreamEvent, StreamHeader, StreamIndex, StreamType}, tasks::simulation::Checkpoint};
 
 #[derive(Debug)]
 pub struct SharedFlash<S> {
@@ -62,12 +63,98 @@ impl<E> core::fmt::Display for StorageRangeError<E> {
     }
 }
 
-#[derive(Debug)]
+#[derive(Debug, Clone)]
 pub struct StorageRange<S> {
     storage: S,
     start: usize,
     end: usize,
-    offset: usize
+    offset: usize,
+    reset_pos: usize
+}
+
+impl<S> Checkpoint for StorageRange<S> {
+    fn checkpoint(&mut self) {
+        self.reset_pos = self.offset;
+    }
+
+    fn rollback(&mut self) {
+        self.offset = self.reset_pos;
+    }
+}
+
+impl<S> StorageRange<S> {
+    pub fn inner(&self) -> &S {
+        &self.storage
+    }
+
+    pub fn inner_mut(&mut self) -> &mut S {
+        &mut self.storage
+    }
+
+    pub fn abs_start(&self) -> usize {
+        self.start
+    }
+
+    pub fn abs_end(&self) -> usize {
+        self.end
+    }
+}
+
+impl<E: core::fmt::Debug> embedded_io::Error for StorageRangeError<E> {
+    fn kind(&self) -> embedded_io::ErrorKind {
+        ErrorKind::Other
+    }
+}
+
+impl<S: ReadStorage<Error = E>, E: core::fmt::Debug> ErrorType for StorageRange<S> {
+    type Error = StorageRangeError<E>;
+}
+
+impl<S: Storage<Error = E> + ReadStorage<Error = E>, E: core::fmt::Debug> Write for StorageRange<S> {
+    fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
+        let pos = self.start + self.offset;
+        trace!("write {:#02x}:{:#02x} -> {:#02x}:{:#02x}", self.start, self.end, pos, pos + buf.len());
+        if pos > self.end {
+            Err(StorageRangeError::OutOfData)
+        } else {
+            assert!(pos + buf.len() <= self.end);
+            match self.storage.write(pos as u32, buf) {
+                Ok(_) => {
+                    self.offset += buf.len();
+                    Ok(buf.len())
+                },
+                Err(err) => Err(StorageRangeError::Storage(err))
+            }
+        }
+    }
+
+    fn flush(&mut self) -> Result<(), Self::Error> {
+        Ok(())
+    }
+}
+
+impl<S: ReadStorage<Error = E>, E: core::fmt::Debug> Read for StorageRange<S> {
+    fn read(&mut self, buf: &mut [u8]) -> Result<usize, Self::Error> {
+        let pos = self.start + self.offset;
+        trace!("read_exact_buf {:#02x}:{:#02x} -> {:#02x}:{:#02x}", self.start, self.end, pos, pos + buf.len());
+        if pos > self.end {
+            Err(StorageRangeError::OutOfData)
+        } else {
+            let remaining = self.end - pos;
+            let max_read = buf.len().min(remaining);
+            if max_read == 0 {
+                return Err(StorageRangeError::OutOfData)
+            }
+            assert!(pos + buf.len() <= self.end);
+            match self.storage.read(pos as u32, &mut buf[..max_read]) {
+                Ok(_) => {
+                    self.offset += max_read;
+                    Ok(max_read)
+                },
+                Err(err) => Err(StorageRangeError::Storage(err))
+            }
+        }
+    }
 }
 
 impl<S: ReadStorage> StorageRange<S> {
@@ -77,7 +164,8 @@ impl<S: ReadStorage> StorageRange<S> {
             storage,
             start,
             end,
-            offset: 0
+            offset: 0,
+            reset_pos: 0
         }
     }
 
@@ -98,6 +186,15 @@ impl<S: ReadStorage> StorageRange<S> {
         }
     }
 
+    pub fn seek_abs(&mut self, pos: usize) -> Result<(), StorageRangeError<S::Error>> {
+        self.offset = pos;
+        if self.offset > self.end {
+            Err(StorageRangeError::OutOfData)
+        } else {
+            Ok(())
+        }
+    }
+
     pub fn subset(&self, size: usize) -> Result<Self, StorageRangeError<S::Error>> where S: Clone + core::fmt::Debug {
         trace!("subset {:#02x}:{:#02x} -> {:#02x}:{:#02x}", self.start, self.end, self.start + self.offset, self.start + self.offset + size);
         if self.start + self.offset + size > self.end {
@@ -107,7 +204,8 @@ impl<S: ReadStorage> StorageRange<S> {
                 storage: self.storage.clone(),
                 start: self.offset + self.start,
                 end: self.start + self.offset + size,
-                offset: 0
+                offset: 0,
+                reset_pos: 0
             })
         }
     }
@@ -153,43 +251,3 @@ impl<S: Storage> RmpWrite for StorageRange<S> where S::Error: core::fmt::Debug +
         }
     }
 }
-
-pub struct SimDataRecorder<S> {
-    storage: StorageRange<S>,
-    last_stamp: Instant
-}
-
-impl<S: Storage + Clone> SimDataRecorder<S> where S::Error: core::fmt::Debug + 'static {
-    pub fn open(storage: S, partitions: PartitionTable<'_>) -> Result<Self, SimDataError<ValueWriteError<StorageRangeError<S::Error>>>> {
-        let partition_type = esp_bootloader_esp_idf::partitions::PartitionType::Data(
-                esp_bootloader_esp_idf::partitions::DataPartitionSubType::Undefined,
-        );
-        info!("Searching for sim data partition");
-        let data_partition = partitions.iter().find(|partition| {
-            partition.partition_type() == partition_type && partition.label_as_str() == "sim"
-        }).ok_or(SimDataError::PartitionNotFound)?;
-
-        let start = data_partition.offset() as usize;
-        let end = data_partition.len() as usize + start;
-        let mut writer = StorageRange::new(storage.clone(), start, end);
-        warn!("Writing new simulation data at {start:#02x}:{end:#02x}");
-        StreamIndex { count: 1 }.write_rmp(&mut writer)?;
-        StreamHeader { id: StreamType::Bundle, size: 0 }.write_rmp(&mut writer)?;
-        Ok(Self {
-            storage: writer,
-            last_stamp: Instant::now()
-        })
-    }
-
-    pub fn write_next<T: EventRecord>(&mut self, event: T) -> Result<(), SimDataError<ValueWriteError<StorageRangeError<S::Error>>>> {
-        BundleEventHeader { id: T::stream_id() }.write_rmp(&mut self.storage)?;
-        let now = Instant::now();
-        let event = StreamEvent {
-            data: event,
-            timecode: (now - self.last_stamp).as_millis() as f64 / 1000.0
-        };
-        event.write_rmp(&mut self.storage)?;
-        self.last_stamp = now;
-        Ok(())
-    }
-}

src/tasks/ble.rs

@@ -3,11 +3,11 @@ use embassy_executor::Spawner;
 use embassy_futures::select::Either;
 use embassy_sync::{blocking_mutex::raw::NoopRawMutex, pubsub::{DynSubscriber, PubSubChannel, Publisher}};
 use esp_radio::ble::controller::BleConnector;
-use static_cell::StaticCell;
+use static_cell::{ConstStaticCell, StaticCell};
 use trouble_host::{prelude::*, types::gatt_traits::FromGattError};
 use log::*;
 
-use crate::{backoff::Backoff, events::Prediction};
+use crate::events::Prediction;
 
 #[gatt_server]
 struct SerialServer {
@@ -96,17 +96,17 @@ async fn client_prediction_task(mut src: DynSubscriber<'static, Prediction>, sin
 static STATIC_RESOURCES: StaticCell<HostResources<DefaultPacketPool, 1, 1>> = StaticCell::new();
 static STATIC_STACK: StaticCell<Stack<'static, ExternalController<BleConnector<'static>, 1>, DefaultPacketPool>> = StaticCell::new();
 static STATIC_SERVER: StaticCell<SerialServer> = StaticCell::new();
-static STATIC_CLIENT_PREDICTIONS: StaticCell<PubSubChannel<NoopRawMutex, Prediction, 5, 1, 1>> = StaticCell::new();
+static STATIC_CLIENT_PREDICTIONS: ConstStaticCell<PubSubChannel<NoopRawMutex, Prediction, 5, 1, 1>> = ConstStaticCell::new(PubSubChannel::new());
 
 #[embassy_executor::task]
 pub async fn ble_task(ble: BleConnector<'static>, predictions: DynSubscriber<'static, Prediction>, spawner: Spawner) {
     info!("Starting BLE stack");
-    let server = STATIC_SERVER.init(SerialServer::new_with_config(GapConfig::Peripheral(PeripheralConfig { name: "Renderbug", appearance: &appearance::light_source::LED_ARRAY })).unwrap());
+    let server = STATIC_SERVER.init_with(|| { SerialServer::new_with_config(GapConfig::Peripheral(PeripheralConfig { name: "Renderbug", appearance: &appearance::light_source::LED_ARRAY })).unwrap() });
     let control: ExternalController<esp_radio::ble::controller::BleConnector<'_>, 1> = ExternalController::new(ble);
-    let stack = STATIC_STACK.init(trouble_host::new(control, STATIC_RESOURCES.init(HostResources::new())));
+    let stack = STATIC_STACK.init_with(|| { trouble_host::new(control, STATIC_RESOURCES.init_with(|| { HostResources::new() })) });
     let Host { mut peripheral, mut runner, .. } = stack.build();
 
-    let client_predictions = STATIC_CLIENT_PREDICTIONS.init(PubSubChannel::new());
+    let client_predictions = STATIC_CLIENT_PREDICTIONS.take();
     spawner.must_spawn(client_prediction_task(predictions, client_predictions.publisher().unwrap()));
 
     // The host task must be started and ticking beore we can start advertising, so we use a join() here instead of a separate task
@@ -114,43 +114,41 @@ pub async fn ble_task(ble: BleConnector<'static>, predictions: DynSubscriber<'st
         runner.run(),
         async {
             loop {
-                let advertiser = Backoff::from_secs(5).forever().attempt(async || {
-                    info!("Starting BLE advertising");
-                    let mut adv_data = [0; 64];
-                    let len = AdStructure::encode_slice(
-                        &[
-                            AdStructure::CompleteLocalName("Renderbug".as_bytes()),
-                            AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
-                            AdStructure::ServiceUuids128(&[
-                                [ // Serial service
-                                    0x6E, 0x40, 0x00, 0x01,
-                                    0xB5, 0xA3,
-                                    0xF3, 0x93,
-                                    0xE0, 0xA9,
-                                    0xE5, 0x0E, 0x24, 0xDC, 0xCA, 0x9E
-                                    //0x9E, 0xCA, 0xDC, 0x24, 0x0E, 0xE5, 0xA9, 0xE0, 0x93, 0xF3, 0xA3, 0xB5,
-                                    //0x01, 0x00, 0x40, 0x6E,
-                                ],
-                                [ // Renderbug mesh service
-                                    0x00, 0x00, 0x00, 0x00,
-                                    0xf4, 0x3a,
-                                    0x48, 0xc4,
-                                    0x84, 0x11,
-                                    0xef, 0xe6, 0xa3, 0x8a, 0x5d, 0x75
-                                ]
-                            ]),
-                            AdStructure::ServiceUuids16(&[
-                                // Location and navigation
-                                [0x18, 0x19]
-                            ])
-                        ],
-                        &mut adv_data[..],
-                    ).unwrap();
-                    peripheral.advertise(
-                        &Default::default(),
-                        Advertisement::ConnectableScannableUndirected { adv_data: &adv_data[..len], scan_data: &[] }
-                    ).await
-                }).await.unwrap();
+                info!("Starting BLE advertising");
+                let mut adv_data = [0; 64];
+                let len = AdStructure::encode_slice(
+                    &[
+                        AdStructure::CompleteLocalName("Renderbug".as_bytes()),
+                        AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
+                        AdStructure::ServiceUuids128(&[
+                            [ // Serial service
+                                0x6E, 0x40, 0x00, 0x01,
+                                0xB5, 0xA3,
+                                0xF3, 0x93,
+                                0xE0, 0xA9,
+                                0xE5, 0x0E, 0x24, 0xDC, 0xCA, 0x9E
+                                //0x9E, 0xCA, 0xDC, 0x24, 0x0E, 0xE5, 0xA9, 0xE0, 0x93, 0xF3, 0xA3, 0xB5,
+                                //0x01, 0x00, 0x40, 0x6E,
+                            ],
+                            [ // Renderbug mesh service
+                                0x00, 0x00, 0x00, 0x00,
+                                0xf4, 0x3a,
+                                0x48, 0xc4,
+                                0x84, 0x11,
+                                0xef, 0xe6, 0xa3, 0x8a, 0x5d, 0x75
+                            ]
+                        ]),
+                        AdStructure::ServiceUuids16(&[
+                            // Location and navigation
+                            [0x18, 0x19]
+                        ])
+                    ],
+                    &mut adv_data[..],
+                ).unwrap();
+                let advertiser = peripheral.advertise(
+                    &Default::default(),
+                    Advertisement::ConnectableScannableUndirected { adv_data: &adv_data[..len], scan_data: &[] }
+                ).await.unwrap();
                 info!("Waiting for connection");
                 match advertiser.accept().await.and_then(|raw| { raw.with_attribute_server(server) }) {
                     Ok(conn) => {

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();
+    }
 }

src/tasks/mod.rs

@@ -5,7 +5,6 @@ pub mod gps;
 pub mod wifi;
 #[cfg(feature="radio")]
 pub mod ble;
-#[cfg(feature="simulation")]
 pub mod simulation;
 #[cfg(feature="demo")]
 pub mod demo;
@@ -14,6 +13,8 @@ pub mod oled_render;
 
 pub mod usb_power;
 
+pub mod sd_card;
+
 // Prediction engines
 pub mod motion;
 

src/tasks/motion.rs

@@ -4,8 +4,6 @@ use embassy_time::{Duration, WithTimeout};
 
 use crate::{ego::engine::BikeStates, events::{Measurement, Prediction}};
 
-const TIMEOUT: Duration = Duration::from_millis(3);
-
 #[embassy_executor::task]
 pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_sink: DynPublisher<'static, Prediction>, recording_sink: DynPublisher<'static, Measurement>) {
     let mut states = BikeStates::default();
@@ -17,23 +15,30 @@ pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_
         match next_measurement {
             Measurement::IMU { accel, gyro } => {
                 states.insert_imu(accel, gyro);
-                states.commit(&prediction_sink).with_timeout(TIMEOUT).await.expect("Could not commit IMU data in time. Is the prediction bus stalled?");
+                states.commit(&prediction_sink).await;
             },
             Measurement::GPS(Some(gps_pos)) => {
                 states.insert_gps(gps_pos);
-                states.commit(&prediction_sink).with_timeout(TIMEOUT).await.expect("Could not commit GPS data in time. Is the prediction bus stalled?");
+                states.commit(&prediction_sink).await;
             },
             Measurement::GPS(None) => {
                 states.has_gps_fix.set(false);
             },
             // FIXME: This needs harmonized with the automatic data timeout from above, somehow?
             Measurement::SensorHardwareStatus(source, state) => {
-                warn!("Sensor {source:?} reports {state:?}!");
-                prediction_sink.publish(Prediction::SensorStatus(source, state)).with_timeout(TIMEOUT).await.expect("Could not update sensor status in time. Is the prediction bus stalled?");
+                debug!("Sensor {source:?} reports {state:?}!");
+                prediction_sink.publish(Prediction::SensorStatus(source, state)).await;
+            },
+            Measurement::ExternalPower(mw) => {
+                info!("Got external power change to {mw:?}");
             },
             Measurement::SimulationProgress(source, duration, pct) => debug!("{source:?} simulation time: {} {} / 255", duration.as_secs(), pct),
-            Measurement::Annotation => ()
+            Measurement::Annotation(msg) => {
+                info!("Annotation: {}", str::from_utf8(&msg).unwrap_or("<non-utf8-data>"));
+            }
+        }
+        if recording_sink.try_publish(next_measurement).is_err() {
+            warn!("Could not publish measurement to recording bus. Is the recording bus stalled?");
         }
-        let _ = recording_sink.try_publish(next_measurement);
     }
 }

src/tasks/mpu.rs

@@ -1,5 +1,3 @@
-use core::cell::RefCell;
-
 use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
 use embassy_sync::blocking_mutex::raw::NoopRawMutex;
 use embassy_sync::channel::DynamicSender;
@@ -13,7 +11,7 @@ use nalgebra::Vector3;
 use crate::events::SensorSource;
 
 use crate::gpio_interrupt::PinInterrupt;
-use crate::{backoff::Backoff, events::Measurement};
+use crate::events::Measurement;
 
 const G: f32 = 9.80665;
 const GYRO_SCALE: GyroFullScale = GyroFullScale::Deg2000;
@@ -21,44 +19,44 @@ const ACCEL_SCALE: AccelFullScale = AccelFullScale::G2;
 
 #[embassy_executor::task]
 pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevice<'static, NoopRawMutex, I2c<'static, Async>>, _interrupt: PinInterrupt<'static>) {
-    let backoff = Backoff::from_millis(5);
-    let busref = RefCell::new(Some(bus));
+    warn!("Initializing connection to MPU");
+    events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::AcquiringFix)).await;
 
-    backoff.forever().attempt::<_, (), ()>(async || {
-        events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Offline)).await;
-        let mut sensor = backoff.forever().attempt(async || {
-            warn!("Initializing connection to 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) => {
-                    events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::AcquiringFix)).await;
-                    match backoff.attempt(async || { mpu_init(&mut sensor).await }).await {
-                        Err(_) => {
-                            busref.replace(Some(sensor.release()));
-                            Err(())
-                        },
-                        Ok(_) => Ok(sensor)
-                    }
-                }
-            }
-        }).await?;
+    let mut sensor = match Mpu6050::new(bus, Address::default()).await {
+        Err(err) => {
+            error!("Could not connect to MPU: {err:?}");
+            events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Offline)).await;
+            return;
+        },
+        Ok(sensor) => sensor
+    };
 
-        let sensor_ref = &mut sensor;
+    loop {
+        if let Err(err) = mpu_init(&mut sensor).await {
+            error!("Could not initialize MPU: {err:?}");
+            events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Degraded)).await;
+            continue;
+        }
 
         events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Online)).await;
+
         //TODO: Need to read in a constant buffer of accelerometer measurements, which we can then use to determine where "forward" points in the body frame when converting from the sensor frame.
         // From there, we can rotate the body frame into the world frame using gps headings to generate a compass north
         fn lowpass(prev: f32, current: f32, alpha: f32) -> f32 {
             prev + alpha * (current - prev) // alpha in (0,1), small alpha = heavy smoothing
         }
         let mut prev_accel = Vector3::default();
+        let mut degraded = false;
         loop {
-            match backoff.attempt(async || { sensor_ref.motion6().await }).await {
+            match sensor.motion6().await {
                 Ok((accel_data, gyro_data)) => {
 
+                    if degraded {
+                        warn!("Reconnected to MPU");
+                        events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Online)).await;
+                        degraded = false;
+                    }
+
                     // Apply the initial alignment correction to put it into the body frame where X is forward
                     let scaled = accel_data.scaled(ACCEL_SCALE);
                     let adjusted = Vector3::new(
@@ -91,12 +89,19 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
                 },
                 Err(e) => {
                     error!("Failed to read MPU motion data! {e:?}");
-                    busref.replace(Some(sensor.release()));
-                    return Err(());
+                    if !degraded {
+                        events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Degraded)).await;
+                        degraded = true;
+                    } else {
+                        error!("MPU connection is degraded, and failed to read data again. Attempting to re-initialize connection...");
+                        events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::AcquiringFix)).await;
+                        break;
+                    }
+                    Timer::after_millis(10).await;
                 }
             };
         }
-    }).await.ok();
+    }
 }
 
 async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, NoopRawMutex, I2c<'static, Async>>>) -> Result<(), Error<I2cDevice<'static, NoopRawMutex, I2c<'static, Async>>>> {
@@ -105,10 +110,9 @@ async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, NoopRawMutex, I2c<'sta
         Ok(())
     } else {
         let mut delay = Delay;
-        let backoff = Backoff::from_millis(10);
         info!("Initializing DMP");
         let start = Instant::now();
-        backoff.attempt(async || { sensor.initialize_dmp(&mut delay).await }).await?;
+        sensor.initialize_dmp(&mut delay).await?;
         info!("DMP initialized in {}ms", start.as_millis());
         Ok(())
     }

src/tasks/oled.rs

@@ -20,7 +20,7 @@ pub type LockedUniforms = Arc<Mutex<CriticalSectionRawMutex, OledUniforms>>;
 
 pub struct OledUI<S: Surface + core::fmt::Debug> {
     overlay: S,
-    controls: DisplayControls,
+    controls: DisplayControls<'static>,
     uniforms: LockedUniforms
 }
 
@@ -32,7 +32,7 @@ impl Shader<OledUniforms, Matrix2DSpace, BinaryColor> for OverlayShader {
 }
 
 impl<S: core::fmt::Debug + Surface<CoordinateSpace = Matrix2DSpace, Pixel = BinaryColor, Uniforms = OledUniforms>> OledUI<S> {
-    pub fn new<SS: Surfaces<Surface = S>>(surfaces: &mut SS, controls: DisplayControls, uniforms: LockedUniforms) -> Self where SS::Error: core::fmt::Debug {
+    pub fn new<SS: Surfaces<Surface = S>>(surfaces: &mut SS, controls: DisplayControls<'static>, uniforms: LockedUniforms) -> Self where SS::Error: core::fmt::Debug {
         Self {
             overlay: SurfaceBuilder::build(surfaces)
                 .rect(Rectangle::everything())
@@ -94,7 +94,6 @@ impl<S: core::fmt::Debug + Surface<CoordinateSpace = Matrix2DSpace, Pixel = Bina
 
 #[embassy_executor::task]
 pub async fn oled_ui(mut events: DynSubscriber<'static, Prediction>, mut ui: OledUI<OledSurface>) {
-    
     ui.screen_transition(Screen::Bootsplash).await;
     Timer::after_secs(3).await;
     ui.screen_transition(Screen::Home).await;
@@ -102,5 +101,4 @@ pub async fn oled_ui(mut events: DynSubscriber<'static, Prediction>, mut ui: Ole
     loop {
         ui.on_event(events.next_message_pure().await).await;
     }
-
 }

src/tasks/oled_render.rs

@@ -12,7 +12,7 @@ use crate::{backoff::Backoff, graphics::ssd1306::SsdOutput, tasks::oled::{Locked
 pub async fn oled_render(mut output: SsdOutput, mut surfaces: OledUiSurfacePool, uniforms: LockedUniforms) {
     warn!("Starting OLED rendering task");
     Backoff::from_secs(1).forever().attempt::<_, (), DisplayError>(async || {
-        const FPS: u64 = 30;
+        const FPS: u64 = 15;
         const RENDER_BUDGET: Duration = Duration::from_millis(1000 / FPS);
 
         const ANIMATION_TPS: u64 = 30;
@@ -35,7 +35,7 @@ pub async fn oled_render(mut output: SsdOutput, mut surfaces: OledUiSurfacePool,
             if frame_time < RENDER_BUDGET {
                 Timer::after(RENDER_BUDGET - frame_time).await;
             } else {
-                //warn!("OLED Frame took too long to render! {}ms", frame_time.as_millis());
+                warn!("OLED Frame took too long to render! {}ms", frame_time.as_millis());
             }
         }
     }).await.unwrap();

src/tasks/render.rs

@@ -9,16 +9,13 @@ use figments_render::gamma::GammaCurve;
 use figments_render::output::{GammaCorrected, OutputAsync};
 use log::*;
 
-use crate::graphics::display::NUM_PIXELS;
+use crate::graphics::display::{NUM_PIXELS, RenderState};
 use crate::{graphics::display::{BikeOutput, DisplayControls, Uniforms}, tasks::ui::UiSurfacePool};
 
-const POWER_MA : u32 = 300;
-const POWER_VOLTS : u32 = 5;
-const MAX_POWER_MW : u32 = POWER_VOLTS * POWER_MA;
 static SPI_BUFFERS: static_cell::ConstStaticCell<DmaBuffers<u8, {NUM_PIXELS * 12 + 140}>> = static_cell::ConstStaticCell::new(DmaBuffers::new(0));
 
 #[embassy_executor::task]
-pub async fn render(spi: AnySpi<'static>, dma: AnyGdmaChannel<'static>, gpio: AnyPin<'static>, mut surfaces: UiSurfacePool, mut safety_surfaces: UiSurfacePool, mut controls: DisplayControls, mut wdt: Wdt<esp_hal::peripherals::TIMG0<'static>>) {
+pub async fn render(spi: AnySpi<'static>, dma: AnyGdmaChannel<'static>, gpio: AnyPin<'static>, mut surfaces: UiSurfacePool, mut safety_surfaces: UiSurfacePool, mut controls: DisplayControls<'static>, mut wdt: Wdt<esp_hal::peripherals::TIMG0<'static>>) {
     info!("Starting rendering task");
 
     let buffers = SPI_BUFFERS.take();
@@ -40,11 +37,10 @@ pub async fn render(spi: AnySpi<'static>, dma: AnyGdmaChannel<'static>, gpio: An
         ..Uniforms::default()
     };
 
-    let mut output = BikeOutput::new(target, MAX_POWER_MW, controls.clone());
+    let mut output = BikeOutput::new(target, controls.clone());
     output.set_gamma(GammaCurve::new(1.3));
 
     info!("Rendering started! {}ms since boot", Instant::now().as_millis());
-    controls.notify_render_is_running(true);
 
     let mut requested_fps= controls.fps() as u64;
     let mut render_budget = Duration::from_millis(1000 / requested_fps);
@@ -77,15 +73,15 @@ pub async fn render(spi: AnySpi<'static>, dma: AnyGdmaChannel<'static>, gpio: An
 
         if !controls.is_on() {
             warn!("Renderer is sleeping zzzz");
-            controls.notify_render_is_running(false);
             output.blank();
             output.commit_async().await.expect("Failed to commit low power frame");
             wdt.disable();
-            controls.wait_until_display_is_turned_on().await;
+            controls.ack().await;
+            controls.wait_for_state(RenderState::On).await;
+            controls.ack().await;
             wdt.feed();
             wdt.enable();
             warn!("Renderer is awake !!!!");
-            controls.notify_render_is_running(true);
         }
 
         // Apply the FPS cap where we sleep if we are rendering fast enough

src/tasks/safetyui.rs

@@ -16,11 +16,11 @@ pub struct SafetyUi<S: Surface> {
 
     // The overlay covers everything and is used to implement a power-on and power-off animation.
     overlay: AnimatedSurface<S>,
-    display: DisplayControls
+    display: DisplayControls<'static>
 }
 
 impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pixel = Rgba<u8>>> SafetyUi<S> {
-    pub fn new<SS: Surfaces<Surface = S>>(surfaces: &mut SS, display: DisplayControls) -> Self where SS::Error: Debug {
+    pub fn new<SS: Surfaces<Surface = S>>(surfaces: &mut SS, display: DisplayControls<'static>) -> Self where SS::Error: Debug {
         let ret = Self {
             overlay: SurfaceBuilder::build(surfaces)
                 .rect(Rectangle::everything())
@@ -68,7 +68,7 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
         self.display.set_brightness(Fract8::MIN);
         self.display.set_on(true);
         // Wait for the renderer to start running again
-        self.display.wait_until_render_is_running().await;
+        self.display.wait_for_ack(RenderState::On).await;
 
         trace!("Fading in brightness with overlay={:?}", self.overlay);
         self.overlay.set_opacity(Fract8::MAX);

src/tasks/sd_card.rs

@@ -0,0 +1,154 @@
+use core::cell::RefCell;
+
+use embassy_sync::pubsub::DynSubscriber;
+use embedded_hal_bus::spi::RefCellDevice;
+use embedded_sdmmc::{Mode, SdCard, TimeSource, VolumeIdx, VolumeManager};
+use esp_hal::{delay::Delay, gpio::{AnyPin, Level, Output}, spi::{master::{AnySpi, Config, Spi}}, time::Rate};
+use rmp::encode::{RmpWrite, RmpWriteErr};
+
+use crate::{events::Measurement, gpio_interrupt::PinInterrupt, simdata::IMUReading, storage::SimDataRecorder};
+use log::*;
+
+struct SdTimeSource;
+
+// TODSO: We need a whole time keeping stack that can sync from GPS or NTP
+impl TimeSource for SdTimeSource {
+    fn get_timestamp(&self) -> embedded_sdmmc::Timestamp {
+        embedded_sdmmc::Timestamp::from_fat(0, 0)
+    }
+}
+
+#[allow(clippy::single_match)]
+#[embassy_executor::task]
+pub async fn sdcard_task(
+    spi: AnySpi<'static>,
+    sck: AnyPin<'static>,
+    mosi: AnyPin<'static>,
+    miso: AnyPin<'static>,
+    cs: Output<'static>,
+    sd_detect_interrupt: PinInterrupt<'static>,
+    mut recording_stream: DynSubscriber<'static, Measurement>
+) {
+
+    let sd_spi = Spi::new(spi, Config::default().with_frequency(Rate::from_khz(400)).with_mode(esp_hal::spi::Mode::_0))
+        .unwrap()
+        .with_sck(sck)
+        .with_mosi(mosi)
+        .with_miso(miso)
+        .into_async();
+
+    let sd_ref = RefCell::new(sd_spi);
+    let sd_device = RefCellDevice::new(&sd_ref, cs, Delay::new()).unwrap();
+    let mut sd_card = SdCard::new(sd_device, Delay::new());
+    
+    loop {
+        match sd_detect_interrupt.wait_for_interrupt().await {
+            Level::High => {
+                info!("SD card inserted!");
+                sd_detect_interrupt.listen();
+            },
+            Level::Low => {
+                info!("SD card removed!");
+                sd_detect_interrupt.listen();
+                continue;
+            }
+        }
+
+        sd_card.mark_card_uninit();
+
+
+        match sd_card.get_card_type() {
+            Some(card_type) => info!("SD card detected: {:?}", card_type),
+            None => {
+                error!("Failed to determine SD card type!");
+                continue;
+            }
+        }
+
+        let volumes = VolumeManager::new(sd_card, SdTimeSource);
+        let vol = volumes.open_volume(VolumeIdx(0)).unwrap();
+        let rootfs = vol.open_root_dir().unwrap();
+
+        let stream = rootfs.open_file_in_dir("renderbug-sensors.rmp",Mode::ReadWriteCreateOrTruncate).unwrap();
+        let rmp_stream = EmbeddedRmp(stream);
+
+        /*let mut recorder = SimDataStream::open(rmp_stream, StreamType::Bundle);
+
+        loop {
+            // FIXME: THis chunk could really go into an impl From<Measurement> for EventRecord
+            match recording_stream.next_message_pure().await {
+                Measurement::IMU { accel, gyro } => {
+                    let reading = IMUReading {
+                        accel_x: accel.x as f64,
+                        accel_y: accel.y as f64,
+                        accel_z: accel.z as f64,
+                        gyro_x: gyro.x as f64,
+                        gyro_y: gyro.y as f64,
+                        gyro_z: gyro.z as f64
+                    };
+                    /*if recorder.write_next(reading).is_err() {
+                        error!("Failed to write IMU reading to SD card");
+                        break;
+                    }*/
+                    info!("Wrote IMU to SD card");
+                },
+                _ => ()
+            }
+        }
+
+        drop(recorder);*/
+        drop(rmp_stream);
+        drop(rootfs);
+        drop(vol);
+
+        sd_card = volumes.free().0;
+    }
+}
+
+struct EmbeddedRmp<W>(W);
+
+#[derive(Debug)]
+struct EmbeddedRmpError<T>(T);
+
+impl<T: core::fmt::Debug + 'static> RmpWriteErr for EmbeddedRmpError<T> {
+}
+impl<T: core::fmt::Debug + 'static> RmpReadErr for EmbeddedRmpError<T> {
+}
+
+impl<T> core::fmt::Display for EmbeddedRmpError<T> where T: core::fmt::Debug {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        write!(f, "{:?}", self.0)
+    }
+}
+
+impl<W: embedded_io::Write> RmpWrite for EmbeddedRmp<W> where W::Error: core::fmt::Debug + 'static {
+    type Error = EmbeddedRmpError<W::Error>;
+
+    fn write_bytes(&mut self, buf: &[u8]) -> Result<(), Self::Error> {
+        self.0.write(buf).map_err(|e| { EmbeddedRmpError(e)})?;
+        Ok(())
+    }
+}
+
+impl<W: embedded_io::Read> RmpRead for EmbeddedRmp<W> where W::Error: core::fmt::Debug + 'static {
+    type Error = EmbeddedRmpError<ReadExactError<W::Error>>;
+
+    fn read_exact_buf(&mut self, buf: &mut [u8]) -> Result<(), Self::Error> {
+        self.0.read_exact(buf).map_err(|e| { EmbeddedRmpError(e) })?;
+        Ok(())
+    }
+}
+
+impl<W: embedded_io::Write> embedded_io::ErrorType for EmbeddedRmp<W> {
+    type Error = W::Error;
+}
+
+impl<W: embedded_io::Write> embedded_io::Write for EmbeddedRmp<W> {
+    fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
+        self.0.write(buf)
+    }
+
+    fn flush(&mut self) -> Result<(), Self::Error> {
+        self.0.flush()
+    }
+}

src/tasks/simulation.rs

@@ -9,18 +9,46 @@ use esp_storage::FlashStorage;
 use figments::liber8tion::interpolate::Fract8;
 use nalgebra::{Vector2, Vector3};
 use log::*;
-use rmp::{decode::ValueReadError, encode::ValueWriteError};
+use rmp::{decode::{RmpRead, RmpReadErr, ValueReadError}, encode::{RmpWrite, RmpWriteErr, ValueWriteError}};
 
 use crate::{Breaker, events::{Measurement, SensorSource, SensorState}, simdata::{AnnotationReading, BundleEventHeader, EventRecord, EventStreamHeader, GPSReading, IMUReading, RmpData, SimDataError, StreamEvent, StreamHeader, StreamIndex, StreamType}, storage::{SharedFlash, StorageRange, StorageRangeError}};
 
 pub struct SimDataTable<S> {
-    reader: StorageRange<S>,
+    reader: S,
     count: usize,
     index: usize
 }
 
-impl<S: ReadStorage> SimDataTable<S> where S::Error: core::fmt::Debug + 'static {
-    pub fn open(storage: S, partitions: PartitionTable<'_>) -> Result<Self, SimDataError<S>> {
+impl<S: RmpRead<Error = E>, E: RmpReadErr + RmpWriteErr> SimDataTable<S> {
+    pub fn open(mut reader: S) -> Result<Self, SimDataError<E>> {
+        if let Ok(index) = StreamIndex::from_rmp(&mut reader) {
+            info!("Found stream index: {index:?}");
+            Ok(Self {
+                reader,
+                count: index.count,
+                index: 0
+            })
+        } else {
+            Err(SimDataError::StreamIndexMissing)
+        }
+    }
+}
+
+impl<S: RmpWrite<Error = E>, E: RmpReadErr + RmpWriteErr> SimDataTable<S> {
+    pub fn create(mut writer: S) -> Result<Self, SimDataError<E>> {
+        // FIXME: The stream header should use a fixed size integer for the count, instead of the dynamic array length type
+        info!("Writing new stream index");
+        StreamIndex { count: 0 }.write_rmp(&mut writer)?;
+        Ok(Self {
+            reader: writer,
+            count: 0,
+            index: 0
+        })
+    }
+}
+
+impl<S: ReadStorage<Error = E>, E: core::fmt::Debug + 'static> SimDataTable<StorageRange<S>> {
+    pub fn find_partition(storage: S, partitions: PartitionTable<'_>) -> Result<StorageRange<S>, SimDataError<StorageRangeError<E>>> {
         let partition_type = esp_bootloader_esp_idf::partitions::PartitionType::Data(
                 esp_bootloader_esp_idf::partitions::DataPartitionSubType::Undefined,
         );
@@ -32,29 +60,41 @@ impl<S: ReadStorage> SimDataTable<S> where S::Error: core::fmt::Debug + 'static
         let start = data_partition.offset() as usize;
         let end = data_partition.len() as usize + start;
         info!("Opening simulation data at {start:#02x}:{end:#02x}");
-        let mut reader = StorageRange::new(storage, start, end);
-        if let Ok(index) = StreamIndex::from_rmp(&mut reader) {
-            info!("Found stream index: {index:?}");
-            Ok(Self {
-                reader,
-                count: index.count,
-                index: 0
-            })
-        } else {
-            error!("Stream index is missing! Have you flashed the partition yet?");
-            Err(SimDataError::StreamIndexMissing)
-        }
+        Ok(StorageRange::new(storage, start, end))
     }
 }
 
-impl<S: ReadStorage + Clone + core::fmt::Debug> Iterator for SimDataTable<S> where S::Error: core::fmt::Debug + 'static {
-    type Item = SimDataReader<S>;
+impl<S: Storage<Error = E> + Clone + core::fmt::Debug, E: core::fmt::Debug + 'static> SimDataTable<StorageRange<S>> {
+    pub fn append_new_stream(&mut self, streamid: StreamType) -> Result<StorageRange<S>, SimDataError<StorageRangeError<E>>> {
+        let stream_size = self.reader.capacity() - self.reader.pos() - 6;
+        if stream_size < 1024 {
+            error!("Not enough space left in the simulation data partition to create a new stream!");
+            return Err(SimDataError::NotEnoughSpace);
+        }
+
+        StreamHeader { id: streamid, size: stream_size }.write_rmp(&mut self.reader)?;
+        self.count += 1;
+        self.index += 1;
+        let reset_pos = self.reader.pos();
+        self.reader.seek_abs(0).unwrap();
+        StreamIndex { count: self.count }.write_rmp(&mut self.reader)?;
+        self.reader.seek_abs(reset_pos).unwrap();
+
+        info!("Creating new stream id={streamid:?} capacity={stream_size:#02x} range={:#02x}:{:#02x}", self.reader.abs_start() + self.reader.pos(), self.reader.abs_start() + stream_size);
+
+        Ok(self.reader.subset(stream_size).unwrap())
+    }
+}
+
+impl<S: ReadStorage<Error = E> + Clone + core::fmt::Debug, E: core::fmt::Debug + 'static> Iterator for SimDataTable<StorageRange<S>> {
+    type Item = (StreamHeader, StorageRange<S>);
 
     fn next(&mut self) -> Option<Self::Item> {
         if self.index >= self.count {
             None
         } else {
             loop {
+                let reset_pos = self.reader.pos();
                 match StreamHeader::from_rmp(&mut self.reader) {
                     Ok(header) => {
                         info!("Found stream header: {header:?}");
@@ -66,7 +106,7 @@ impl<S: ReadStorage + Clone + core::fmt::Debug> Iterator for SimDataTable<S> whe
                                 });
                                 self.index += 1;
                                 debug!("Found header={header:?}");
-                                return Some(SimDataReader::open(sensor_reader, header.id));
+                                return Some((header, sensor_reader));
                             },
                             err => {
                                 error!("Could not open the next simulation data chunk: {err:?}");
@@ -83,6 +123,8 @@ impl<S: ReadStorage + Clone + core::fmt::Debug> Iterator for SimDataTable<S> whe
                     }
                     Err(err) => {
                         error!("Read error while reading next chunk in simulation stream table {err:?}");
+                        // Reset back to right before we tried reading the stream header, so that append_new_stream() places it correctly.
+                        self.reader.seek_abs(reset_pos).unwrap();
                         return None;
                     }
                 }
@@ -91,10 +133,11 @@ impl<S: ReadStorage + Clone + core::fmt::Debug> Iterator for SimDataTable<S> whe
     }
 }
 
-pub struct SimDataReader<S> {
-    reader: StorageRange<S>,
+pub struct SimDataStream<S> {
+    reader: S,
     srcid: StreamType,
-    runtime: Duration,
+    last_stamp: Instant,
+    //runtime: Duration,
     event_count: usize,
     index: usize
 }
@@ -116,60 +159,117 @@ impl From<GPSReading> for Measurement {
 
 impl From<AnnotationReading> for Measurement {
     fn from(value: AnnotationReading) -> Self {
-        warn!("ANNOTATION: {}", core::str::from_utf8(&value.buf).unwrap());
-        Measurement::Annotation
+        Measurement::Annotation(value.buf)
     }
 }
 
-impl<S: ReadStorage> SimDataReader<S> where S::Error: core::fmt::Debug + 'static {
-    pub fn open(mut reader: StorageRange<S>, stream_type: StreamType) -> Self {
-        debug!("Opening {stream_type:?} sim data chunk");
-        let event_count = if stream_type != StreamType::Bundle { EventStreamHeader::from_rmp(&mut reader).unwrap().count } else { usize::MAX };
-        debug!("Found {event_count} events!");
+impl<S> SimDataStream<S> {
+    pub fn srcid(&self) -> StreamType {
+        self.srcid
+    }
+}
+
+pub trait Checkpoint {
+    fn checkpoint(&mut self);
+    fn rollback(&mut self);
+}
+
+impl<S: Checkpoint + RmpRead<Error = E>, E: RmpWriteErr + RmpReadErr> SimDataStream<S> {
+    pub fn open(mut reader: S, stream_type: StreamType) -> Self {
+        info!("Opening {stream_type:?} sim data chunk");
+        let event_count = EventStreamHeader::from_rmp(&mut reader).unwrap().count;
+        info!("Found {event_count} events!");
         Self {
             reader,
             srcid: stream_type,
-            runtime: Default::default(),
+            //runtime: Default::default(),
+            last_stamp: Instant::now(),
             event_count,
             index: 0
         }
     }
 
-    pub fn srcid(&self) -> StreamType {
-        self.srcid
-    }
-
-    async fn read_next_event<T: EventRecord + Into<Measurement>>(&mut self) -> Result<Measurement, SimDataError<ValueReadError<StorageRangeError<S::Error>>>> {
+    fn read_next_event<T: EventRecord + Into<Measurement>>(&mut self) -> Result<(Duration, Measurement), SimDataError<E>> {
         let event = StreamEvent::<T>::from_rmp(&mut self.reader)?;
         let delay = embassy_time::Duration::from_millis((event.timecode * 1000.0) as u64);
-        self.runtime += delay;
-        info!("waiting {delay}");
-        Timer::after(delay).await;
-        Ok(event.data.into())
+        Ok((delay, event.data.into()))
     }
 
-    pub async fn read_next(&mut self) -> Result<Option<Measurement>, SimDataError<ValueReadError<StorageRangeError<S::Error>>>> {
+    pub fn read_next(&mut self) -> Result<Option<(Duration, Measurement)>, SimDataError<E>> {
         if self.index < self.event_count {
             self.index += 1;
+            self.reader.checkpoint();
             let next_id = match self.srcid {
-                StreamType::Bundle => BundleEventHeader::from_rmp(&mut self.reader)?.id,
+                StreamType::Bundle => BundleEventHeader::from_rmp(&mut self.reader).inspect_err(|_| { self.reader.rollback() })?.id,
                 _ => self.srcid
             };
             // The read_* functions can only ever return a valid result, or a data/reading error, so we map them into a Some()
-            Ok(Some(match next_id {
-                StreamType::IMU => self.read_next_event::<IMUReading>().await?,
-                StreamType::GPS => self.read_next_event::<GPSReading>().await?,
-                StreamType::Annotations => self.read_next_event::<AnnotationReading>().await?,
+            let (delay, evt) = match next_id {
+                StreamType::IMU => self.read_next_event::<IMUReading>().inspect_err(|_| { self.reader.rollback() })?,
+                StreamType::GPS => self.read_next_event::<GPSReading>().inspect_err(|_| { self.reader.rollback() })?,
+                StreamType::Annotations => self.read_next_event::<AnnotationReading>().inspect_err(|_| { self.reader.rollback() })?,
                 srcid => unimplemented!("{srcid:?} is not a simulatable sensor yet!")
-            }))
+            };
+            Ok(Some((delay, evt)))
+        } else {
+            Ok(None)
+        }
+    }
+
+    pub async fn read_next_and_wait(&mut self) -> Result<Option<Measurement>, SimDataError<E>> {
+        if let Some((delay, evt)) = self.read_next()? {
+            trace!("waiting {delay}");
+            Timer::after(delay).await;
+            self.last_stamp = Instant::now();
+            Ok(Some(evt))
         } else {
             Ok(None)
         }
     }
 }
 
+impl<S: Storage<Error = SE>, SE: core::fmt::Debug + 'static> SimDataStream<StorageRange<S>> {
+    pub fn write_next<T: EventRecord>(&mut self, event: T) -> Result<(), SimDataError<StorageRangeError<SE>>> {
+        BundleEventHeader { id: T::stream_id() }.write_rmp(&mut self.reader)?;
+        let now = Instant::now();
+        let event = StreamEvent {
+            data: event,
+            timecode: (now - self.last_stamp).as_millis() as f64 / 1000.0
+        };
+        event.write_rmp(&mut self.reader)?;
+        self.last_stamp = now;
+        self.event_count = self.event_count.checked_add(1).ok_or(SimDataError::NotEnoughSpace)?;
+
+        self.write_header()?;
+        Ok(())
+    }
+
+    fn write_header(&mut self) -> Result<(), SimDataError<StorageRangeError<SE>>> {
+        let reset_pos = self.reader.pos();
+        self.reader.seek_abs(0).unwrap();
+        trace!("Writing event stream header with count={} to {:#x}", self.event_count, self.reader.abs_start());
+        EventStreamHeader { count: self.event_count }.write_rmp(&mut self.reader)?;
+        self.reader.seek_abs(reset_pos).unwrap();
+
+        Ok(())
+    }
+
+    pub fn create(mut storage: StorageRange<S>, stream_type: StreamType) -> Self {
+        storage.seek_abs(0).unwrap();
+        EventStreamHeader { count: 0 }.write_rmp(&mut storage).unwrap();
+        Self {
+            reader: storage,
+            srcid: stream_type,
+            //runtime: Default::default(),
+            last_stamp: Instant::now(),
+            event_count: 0,
+            index: 0
+        }
+    }
+}
+
 #[embassy_executor::task(pool_size = 3)]
-pub async fn simulation_task(mut reader: SimDataReader<SharedFlash<FlashStorage>>, events: DynamicSender<'static, Measurement>) {
+pub async fn simulation_task(mut reader: SimDataStream<StorageRange<SharedFlash<FlashStorage<'static>>>>, events: DynamicSender<'static, Measurement>) {
     warn!("Starting simulation for {:?}", reader.srcid());
 
     events.send(Measurement::SensorHardwareStatus(SensorSource::Simulation, SensorState::AcquiringFix)).await;
@@ -180,7 +280,7 @@ pub async fn simulation_task(mut reader: SimDataReader<SharedFlash<FlashStorage>
 
     // TODO: SimulationProgress updates
     loop {
-        match reader.read_next().await {
+        match reader.read_next_and_wait().await {
             Ok(Some(next_evt)) => {
                 events.send(next_evt).await;
                 let pct = (idx as f32) / (reader.event_count as f32);

src/tasks/ui.rs

@@ -7,6 +7,19 @@ use log::*;
 
 use crate::{animation::{AnimatedSurface, Animation}, ego::engine::MotionState, events::{Personality, Prediction, Scene, SensorSource, SensorState}, graphics::{display::{SegmentSpace, Uniforms}, shaders::*}};
 
+const NOTIFY_FADE_IN: Animation<Fract8> =   Animation::new().duration(Duration::from_millis(30)) .from(Fract8::MIN).to(Fract8::MAX);
+const NOTIFY_PULSE_OUT: Animation<Fract8> = Animation::new().duration(Duration::from_millis(10)).from(Fract8::MAX).to(Fract8::from_raw(60));
+const NOTIFY_PULSE_IN: Animation<Fract8> =  Animation::new().duration(Duration::from_millis(10)).from(Fract8::MIN).to(Fract8::MAX);
+const NOTIFY_FADE_OUT: Animation<Fract8> =  Animation::new().duration(Duration::from_millis(750))    .from(Fract8::MAX).to(Fract8::MIN);
+
+const FG_FADE_OUT: Animation<Fract8> =      Animation::new().duration(Duration::from_millis(300)).to(Fract8::MIN);
+const BG_FADE_IN: Animation<Fract8> =       Animation::new().duration(Duration::from_millis(300)).to(Fract8::from_raw(128));
+
+const TAIL: Animation<Fract8> =             Animation::new().duration(Duration::from_millis(300)).to(Fract8::from_raw(96));
+const PANELS: Animation<Fract8> =           Animation::new().duration(Duration::from_millis(300)).to(Fract8::from_raw(128));
+const BG: Animation<Fract8> =               Animation::new().duration(Duration::from_millis(300)).to(Fract8::from_raw(32));
+const MOTION: Animation<Fract8> =           Animation::new().duration(Duration::from_secs(1))    .to(Fract8::MIN);
+
 pub struct Ui<S: Surface> {
     // Background layer provides an always-running background for everything to draw on
     background: AnimatedSurface<S>,
@@ -53,25 +66,21 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
     }
 
     pub async fn flash_notification_color(&mut self, color: Rgb<u8>) {
-        let fade_in = Animation::default().from(Fract8::MIN).to(Fract8::MAX).duration(Duration::from_millis(30));
-        let pulse_out = Animation::default().from(Fract8::MAX).to(Fract8::from_raw(60)).duration(Duration::from_millis(100));
-        let pulse_in = Animation::default().from(Fract8::MIN).to(Fract8::MAX).duration(Duration::from_millis(100));
-        let fade_out = Animation::default().from(Fract8::MAX).to(Fract8::MIN).duration(Duration::from_secs(2));
         info!("Flashing notification {color}");
 
         self.notification.set_visible(true);
 
         self.notification.set_shader(Background::from_color(color));
 
-        fade_in.apply([&mut self.notification]).await;
+        NOTIFY_FADE_IN.apply([&mut self.notification]).await;
 
         // Pulse quickly 5 times
         for _ in 0..5 {
-            pulse_out.apply([&mut self.notification]).await;
-            pulse_in.apply([&mut self.notification]).await;
+            NOTIFY_PULSE_OUT.apply([&mut self.notification]).await;
+            NOTIFY_PULSE_IN.apply([&mut self.notification]).await;
         }
 
-        fade_out.apply([&mut self.notification]).await;
+        NOTIFY_FADE_OUT.apply([&mut self.notification]).await;
         self.notification.set_visible(false);
     }
 
@@ -93,26 +102,20 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
         info!("Activating scene {next_scene:?}");
         match next_scene {
             Scene::Ready => {
-                let tail = Animation::default().duration(Duration::from_millis(300)).to(Fract8::from_raw(96));
-                let panels = Animation::default().duration(Duration::from_millis(300)).to(Fract8::from_raw(128));
-                let bg = Animation::default().duration(Duration::from_millis(300)).to(Fract8::from_raw(32));
-                let motion = Animation::default().duration(Duration::from_secs(1)).to(Fract8::MIN);
                 embassy_futures::join::join4(
-                    tail.apply([&mut self.tail]),
-                    panels.apply([&mut self.panels]),
-                    bg.apply([&mut self.background]),
-                    motion.apply([&mut self.motion])
+                    TAIL.apply([&mut self.tail]),
+                    PANELS.apply([&mut self.panels]),
+                    BG.apply([&mut self.background]),
+                    MOTION.apply([&mut self.motion])
                 ).await;
                 self.background.set_shader(Background::default());
             },
             Scene::Idle => {
                 self.background.set_shader(Thinking::default());
-                let fg_fade = Animation::default().duration(Duration::from_millis(300)).to(Fract8::MIN);
-                let bg_fade = Animation::default().duration(Duration::from_millis(300)).to(Fract8::from_raw(128));
 
                 embassy_futures::join::join(
-                    fg_fade.apply([&mut self.tail, &mut self.panels, &mut self.motion]),
-                    bg_fade.apply([&mut self.background])
+                    FG_FADE_OUT.apply([&mut self.tail, &mut self.panels, &mut self.motion]),
+                    BG_FADE_IN.apply([&mut self.background])
                 ).await;
             },
             Scene::Accelerating => {
@@ -128,12 +131,9 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
 
     pub async fn on_event(&mut self, event: Prediction) {
         match event {
-            Prediction::SetPersonality(personality) => match personality {
-                Personality::Active => self.apply_scene(Scene::Ready).await,
-                Personality::Parked => self.apply_scene(Scene::Idle).await,
-                Personality::Waking => self.show().await,
-                _ => ()
-            },
+            Prediction::SetPersonality(Personality::Active) => self.apply_scene(Scene::Ready).await,
+            Prediction::SetPersonality(Personality::Parked) => self.apply_scene(Scene::Idle).await,
+            Prediction::SetPersonality(Personality::Waking) => self.show().await,
             Prediction::Motion { prev: _, next: MotionState::Accelerating } => self.apply_scene(Scene::Accelerating).await,
             Prediction::Motion { prev: _, next: MotionState::Decelerating } => self.apply_scene(Scene::Decelerating).await,
             Prediction::Motion { prev: _, next: MotionState::Steady } => self.apply_scene(Scene::Ready).await,

src/tasks/usb_power.rs

@@ -1,25 +1,52 @@
 use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
-use embassy_sync::blocking_mutex::raw::NoopRawMutex;
+use embassy_sync::{blocking_mutex::raw::NoopRawMutex, channel::DynamicSender};
 use esp_hal::Async;
+use figments_render::power::Milliwatts;
 use log::*;
 
-use crate::{gpio_interrupt::PinInterrupt, tusb320::TUSB320};
+use crate::{events::{Measurement, SensorSource, SensorState}, gpio_interrupt::PinInterrupt, graphics::display::DisplayControls, tusb320::{ActiveCableConnected, DEVICE_ID, InterruptStatus, TUSB320}};
 
 #[embassy_executor::task]
-pub async fn usb_task(bus: I2cDevice<'static, NoopRawMutex, esp_hal::i2c::master::I2c<'static, Async>>, interrupt_pin: PinInterrupt<'static>) {
+pub async fn usb_task(motion: DynamicSender<'static, Measurement>, bus: I2cDevice<'static, NoopRawMutex, esp_hal::i2c::master::I2c<'static, Async>>, interrupt_pin: PinInterrupt<'static>, mut display_control: DisplayControls<'static>) {
     let mut tusb = TUSB320::new(bus);
 
+    motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::AcquiringFix)).await;
     match tusb.get_device_id().await {
-        Ok(val) => {
-            info!("TUSB320 Device ID: {val:?}");
+        Ok(DEVICE_ID) => {
+            info!("TUSB320 connected");
+            motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::Degraded)).await;
         },
-        Err(_) => {
-            error!("Failed to read from TUSB320");
+        Ok(val) => {
+            error!("Unexpected TUSB320 Device ID: {val:?}! Power management is disabled!!");
+            motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::Offline)).await;
+            return;
+        },
+        Err(e) => {
+            error!("Failed to read from TUSB320! Power management is disabled!! {e:?}");
+            motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::Offline)).await;
+            return;
         }
     }
 
     loop {
-        log::info!("Waiting for USB power interrupt...");
+        match tusb.active_cable_connected().read().await.unwrap() {
+            ActiveCableConnected::Connected => {
+                let current_mode = tusb.current_mode_detect().read().await.unwrap();
+                let cur_mw = Milliwatts(current_mode.milliwatts());
+                info!("USB reports current mode: {current_mode:?}, {cur_mw:?} mW");
+                display_control.set_max_power(cur_mw);
+                motion.send(Measurement::ExternalPower(cur_mw)).await;
+                motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::Online)).await;
+            },
+            ActiveCableConnected::Disconnected => {
+                warn!("USB power is disconnected! Setting display power to zero.");
+                // FIXME: We should also send an event through the system to report that the power is down, and the renderer should stop
+                display_control.set_max_power(Milliwatts(0));
+                motion.send(Measurement::ExternalPower(Milliwatts(0))).await;
+                motion.send(Measurement::SensorHardwareStatus(SensorSource::ExternalPower, SensorState::Degraded)).await;
+            }
+        }
         interrupt_pin.wait_for_interrupt().await;
+        tusb.interrupt_status().write(InterruptStatus::Clear).await.ok();
     }
 }

src/tasks/wifi.rs

@@ -1,6 +1,7 @@
-use alloc::string::{String, ToString};
+use alloc::string::ToString;
 use embassy_sync::channel::DynamicSender;
 use embassy_sync::pubsub::DynSubscriber;
+use embassy_sync::watch::{DynReceiver, DynSender};
 use embassy_time::{Duration, Instant, Timer, WithTimeout};
 use esp_hal::rng::Rng;
 use esp_radio::wifi::{ClientConfig, ScanConfig, WifiController, WifiDevice, WifiEvent};
@@ -66,9 +67,19 @@ pub async fn wifi_connect_task(mut wifi: WifiController<'static>, motion: Dynami
     }
 }
 
+#[embassy_executor::task]
+pub async fn location_sampler(mut predictions: DynSubscriber<'static, Prediction>, tx: DynSender<'static, Prediction>) {
+    loop {
+        if let Prediction::Location(coords) = predictions.next_message_pure().await {
+            debug!("New location prediction: {coords}");
+            tx.send(Prediction::Location(coords));
+        }
+    }
+}
+
 // TODO: Wifi task needs to know when there is data to upload, so it only connects when needed.
 #[embassy_executor::task]
-pub async fn http_telemetry_task(mut predictions: DynSubscriber<'static, Prediction>, stack: Stack<'static>, motion: DynamicSender<'static, Measurement>) {
+pub async fn http_telemetry_task(mut latest_prediction: DynReceiver<'static, Prediction>, stack: Stack<'static>, motion: DynamicSender<'static, Measurement>) {
     // TODO: should wait for wifi disconnect event somehow and use that to restart sending the wifi around
 
     let seed = Rng::new().random() as i32;
@@ -91,25 +102,31 @@ pub async fn http_telemetry_task(mut predictions: DynSubscriber<'static, Predict
     let mut last_push = Instant::from_ticks(0);
 
     loop {
-        if let Prediction::Location(coords) = predictions.next_message_pure().await {
-            if stack.is_config_up() {
-                // Only push to HTTP if we have an ip config etc
-                if last_push.elapsed().as_secs() >= 5 || gps_to_local_meters_haversine(&last_location, &coords).norm() >= 10.0 {
-                    last_location = coords;
-                    last_push = Instant::now();
-                    if let Err(e) = Backoff::from_secs(3).attempt(async || {
-                        motion.send(Measurement::SensorHardwareStatus(SensorSource::Cloud, SensorState::AcquiringFix)).await;
-                        push_location(&mut client, coords, Instant::now().as_millis()).await
-                    }).await {
-                        motion.send(Measurement::SensorHardwareStatus(SensorSource::Cloud, SensorState::Offline)).await;
-                        warn!("Could not submit location! {e:?}");
-                    } else {
-                        info!("Location published");
-                        motion.send(Measurement::SensorHardwareStatus(SensorSource::Cloud, SensorState::Online)).await;
-                    }
+        motion.send(Measurement::SensorHardwareStatus(SensorSource::Cloud, SensorState::Offline)).await;
+        stack.wait_config_up().await;
+        info!("Network is up, connecting to nextcloud...");
+        motion.send(Measurement::SensorHardwareStatus(SensorSource::Cloud, SensorState::AcquiringFix)).await;
+
+        if let Err(err) = ping_nextcloud(&mut client).await {
+            error!("Could not ping nextcloud: {err:?}");
+            continue;
+        }
+
+        motion.send(Measurement::SensorHardwareStatus(SensorSource::Cloud, SensorState::Online)).await;
+
+        if let Prediction::Location(coords) = latest_prediction.get().await {
+            // Only push to HTTP if we have an ip config etc
+            if last_push.elapsed().as_secs() >= 5 || gps_to_local_meters_haversine(&last_location, &coords).norm() >= 10.0 {
+                last_location = coords;
+                last_push = Instant::now();
+                if let Err(e) = Backoff::from_secs(3).attempt(async || {
+                    push_location(&mut client, coords, Instant::now().as_millis()).await
+                }).await {
+                    motion.send(Measurement::SensorHardwareStatus(SensorSource::Cloud, SensorState::Offline)).await;
+                    warn!("Could not submit location! {e:?}");
+                } else {
+                    info!("Location published");
                 }
-            } else {
-                motion.send(Measurement::SensorHardwareStatus(SensorSource::Cloud, SensorState::Offline)).await;
             }
         }
     }
@@ -134,3 +151,23 @@ async fn push_location(client: &mut HttpClient<'_, TcpClient<'_, 1, 4096, 4096>,
     debug!("HTTP response: {content}");
     Ok(())
 }
+
+async fn ping_nextcloud(client: &mut HttpClient<'_, TcpClient<'_, 1, 4096, 4096>, DnsSocket<'_>>) -> Result<bool, reqwless::Error> {
+    let mut buffer = [0u8; 4096];
+    let url = "https://nextcloud.malloc.hackerbots.net/nextcloud/index.php/login";
+    info!("Pinging via {url}");
+    let mut http_req = client
+        .request(
+            reqwless::request::Method::HEAD,
+            url,
+        )
+        .await?;
+    let response = http_req.send(&mut buffer).await?;
+    if !response.status.is_client_error() && !response.status.is_server_error() {
+        info!("Nextcloud is online!");
+        Ok(true)
+    } else {
+        warn!("Nextcloud ping returned error status: {:?}", response.status);
+        Ok(false)
+    }
+}

src/tusb320.rs

@@ -1,13 +1,6 @@
-use embassy_embedded_hal::shared_bus::{I2cDeviceError, asynch::i2c::I2cDevice};
-use embassy_sync::blocking_mutex::raw::NoopRawMutex;
-use esp_hal::Async;
-use embedded_hal_async::i2c::I2c;
+use core::marker::PhantomData;
 
-// TODO: rewrite this to only use embedded_hal_async traits, then publish as a crate
-const USB_ADDR: u8 = 0b1100000;
-pub struct TUSB320 {
-    bus: I2cDevice<'static, NoopRawMutex, esp_hal::i2c::master::I2c<'static, Async>>
-}
+use embedded_hal_async::i2c::I2c as AsyncI2c;
 
 #[derive(Clone, Copy, Debug)]
 pub enum RegisterAddress {
@@ -25,307 +18,253 @@ pub enum RegisterAddress {
     DisableRDRP = 0x45
 }
 
-#[derive(Clone, Copy, Debug)]
-pub enum Register {
-    DeviceID,
-    DeviceID1,
-    DeviceID2,
-    DeviceID3,
-    DeviceID4,
-    DeviceID5,
-    DeviceID6,
-    DeviceID7,
-    CurrentAdvertisement,
-    CurrentDetect,
-    AccessoryConnected,
-    ActiveCableConnected,
-    AttachedState,
-    CableOrientation,
-    InterruptStatus,
-    DrpDutyCycle,
-    Debounce,
-    ModeSelect,
-    SoftReset,
-    DisableRDRP
+pub trait RegisterField {
+    fn mask() -> u8;
+    fn shift() -> u8;
+    fn address() -> RegisterAddress;
 }
 
-impl From<Register> for RegisterAddress {
-    fn from(reg: Register) -> Self {
-        match reg {
-            Register::DeviceID => RegisterAddress::DeviceID,
-            Register::DeviceID1 => RegisterAddress::DeviceID1,
-            Register::DeviceID2 => RegisterAddress::DeviceID2,
-            Register::DeviceID3 => RegisterAddress::DeviceID3,
-            Register::DeviceID4 => RegisterAddress::DeviceID4,
-            Register::DeviceID5 => RegisterAddress::DeviceID5,
-            Register::DeviceID6 => RegisterAddress::DeviceID6,
-            Register::DeviceID7 => RegisterAddress::DeviceID7,
+macro_rules! define_register_field {
+    ($name:tt address=$address:tt offset=$shift:tt mask=$bitmask:tt $($value:tt = $valuemask:tt),+) => {
+        #[derive(Debug, Clone, Copy, PartialEq, Eq)]
+        pub enum $name {
+            $($value = $valuemask),+
+        }
 
-            Register::CurrentAdvertisement => RegisterAddress::Status1,
-            Register::CurrentDetect => RegisterAddress::Status1,
-            Register::AccessoryConnected => RegisterAddress::Status1,
-            Register::ActiveCableConnected => RegisterAddress::Status1
-            ,
-            Register::AttachedState => RegisterAddress::Status2,
-            Register::CableOrientation => RegisterAddress::Status2,
-            Register::InterruptStatus => RegisterAddress::Status2,
-            Register::DrpDutyCycle => RegisterAddress::Status2,
+        impl From<u8> for $name {
+            fn from(value: u8) -> Self {
+                match value & 0b11 {
+                    $($valuemask => $name::$value),+,
+                    _ => unreachable!()
+                }
+            }
+        }
 
-            Register::Debounce => RegisterAddress::Status3,
-            Register::ModeSelect => RegisterAddress::Status3,
-            Register::SoftReset => RegisterAddress::Status3,
-            Register::DisableRDRP => RegisterAddress::DisableRDRP
+        impl From<$name> for u8 {
+            fn from(value: $name) -> Self {
+                value as u8
+            }
+        }
+
+        impl RegisterField for $name {
+            fn mask() -> u8 {
+                $bitmask
+            }
+
+            fn shift() -> u8 {
+                $shift
+            }
+
+            fn address() -> RegisterAddress {
+                RegisterAddress::$address
+            }
+        }
+    };
+}
+
+
+define_register_field!{
+    CurrentAdvertisement
+    address=Status1
+    offset=6
+    mask=0b11
+    Default = 0b00,
+    // 1.5A
+    Medium = 0b01,
+    // 3A
+    High = 0b10
+}
+
+define_register_field!{
+    CurrentModeDetect
+    address=Status1
+    offset=4
+    mask=0b11
+    // 500mA
+    Default = 0b00,
+    // 1.5A
+    Medium = 0b01,
+    // 500mA
+    ThroughAccessory = 0b10,
+    // 3A
+    High = 0b11
+}
+
+impl CurrentModeDetect {
+    pub fn milliwatts(&self) -> u32 {
+        match self {
+            CurrentModeDetect::Default => 500,
+            CurrentModeDetect::Medium => 1500,
+            CurrentModeDetect::ThroughAccessory => 500,
+            CurrentModeDetect::High => 3000
         }
     }
 }
 
-pub enum CurrentAdvertisement {
-    /// 500mA / 900mA
-    Default = 0b00,
-    /// 1.5A
-    Medium = 0b01,
-    /// 3A
-    High = 0b10,
-    /// Reserved, do not use
-    Reserved = 0b11
-}
-
-pub enum CurrentModeDetect {
-    /// Default value at startup. TODO: Does this mean 500ma, or just 'no connection'?
-    Default = 0b00,
-    /// 1.5A
-    Medium = 0b01,
-    /// 500ma
-    ThroughAccessory = 0b10,
-    /// 3A
-    High = 0b11,
-}
-
-pub enum AccessoryConnectionState {
+define_register_field!{
+    AccessoryConnected
+    address=Status1
+    offset=1
+    mask=0b111
     None = 0b000,
     Audio = 0b100,
     ThroughAccessory = 0b101,
-    DebugAccessory = 0b110,
-    // All other binary patterns are 'reserved'
-    Reserved
+    DebugAccessory = 0b110
+    // All other patterns are 'reserved' and should not be possible
 }
 
-pub enum AttachedState {
+define_register_field!{
+    ActiveCableConnected
+    address=Status1
+    offset=0
+    mask=0b1
+    Disconnected = 0,
+    Connected = 1
+}
+
+define_register_field!{
+    AttachedState
+    address=Status2
+    offset=6
+    mask=0b11
     Unattached = 0b00,
     Source = 0b01,
     Sink = 0b10,
     Accessory = 0b11
 }
 
-pub enum CableOrientation {
+define_register_field! {
+    CableOrientation
+    address=Status2
+    offset=5
+    mask=0b1
     Normal = 0,
     Flipped = 1
 }
 
-pub enum DrpDutyCycle {
-    /// 30%
+define_register_field! {
+    InterruptStatus
+    address=Status2
+    offset=4
+    mask=0b1
+    Clear = 0,
+    Set = 1
+}
+
+define_register_field! {
+    DrpDutyCycle
+    address=Status2
+    offset=1
+    mask=0b11
+    // 30%
     Default = 0b00,
-    /// 40%
+    // 40%
     Fast = 0b01,
-    /// 50%
+    // 50%
     Faster = 0b10,
-    /// 60%
+    // 60%
     Fastest = 0b11
 }
 
-pub enum Debounce {
-    /// 133ms (default)
+define_register_field! {
+    Debounce
+    address=Status3
+    offset=6
+    mask=0b11
+    // 133ms (default)
     D133 = 0b00,
-    /// 116ms
+    // 116ms
     D116 = 0b01,
-    /// 151ms
+    // 151ms
     D151 = 0b10,
-    /// 168ms
+    // 168ms
     D168 = 0b11
 }
 
-pub enum Mode {
+define_register_field! {
+    ModeSelect
+    address=Status3
+    offset=4
+    mask=0b11
     UsePortPin = 0b00,
     UFP = 0b01,
     DFP = 0b10,
     DRP = 0b11
 }
 
-impl TUSB320 {
-    pub const fn new(bus: I2cDevice<'static, NoopRawMutex, esp_hal::i2c::master::I2c<'static, Async>>) -> Self {
+define_register_field! {
+    DisableRDRP
+    address=DisableRDRP
+    offset=2
+    mask=0b1
+    Enabled = 0,
+    Disabled = 1
+}
+
+pub const DEVICE_ID: [u8; 8] = *b"TUSB320A";
+
+const USB_ADDR_WRITE: u8 = 0b1100000;
+const USB_ADDR_READ: u8 = 0b1100001;
+pub struct TUSB320<T> {
+    bus: T
+}
+
+impl<T: AsyncI2c> TUSB320<T> {
+    pub const fn new(bus: T) -> Self {
         Self { bus }
     }
 
-    pub async fn get_current_advertisement(&mut self) -> Result<CurrentAdvertisement, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::CurrentAdvertisement.into();
-        let val = self.read_address(reg).await? >> 6;
-        match val & 0b11 {
-            0b00 => Ok(CurrentAdvertisement::Default),
-            0b01 => Ok(CurrentAdvertisement::Medium),
-            0b10 => Ok(CurrentAdvertisement::High),
-            0b11 => Ok(CurrentAdvertisement::Reserved),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn set_current_advertisement(&mut self, adv: CurrentAdvertisement) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::CurrentAdvertisement.into();
-        let mut val = self.read_address(reg).await?;
-        let mask = !(0b11 << 6);
-        val &= mask | ((adv as u8) << 6);
-        self.write_address(reg, val).await?;
-        Ok(())
-    }
-
-    pub async fn get_current_mode_detect(&mut self) -> Result<CurrentModeDetect, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::CurrentDetect.into();
-        let val = self.read_address(reg).await? >> 4;
-        match val & 0b11 {
-            0b00 => Ok(CurrentModeDetect::Default),
-            0b01 => Ok(CurrentModeDetect::Medium),
-            0b10 => Ok(CurrentModeDetect::ThroughAccessory),
-            0b11 => Ok(CurrentModeDetect::High),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn get_accessory_connection_state(&mut self) -> Result<AccessoryConnectionState, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::AccessoryConnected.into();
-        let val = self.read_address(reg).await? >> 1;
-        match val & 0b111 {
-            0b000 => Ok(AccessoryConnectionState::None),
-            0b100 => Ok(AccessoryConnectionState::Audio),
-            0b101 => Ok(AccessoryConnectionState::ThroughAccessory),
-            0b110 => Ok(AccessoryConnectionState::DebugAccessory),
-            _ => Ok(AccessoryConnectionState::Reserved)
-        }
-    }
-
-    pub async fn get_cable_detected(&mut self) -> Result<bool, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::ActiveCableConnected.into();
-        let val = self.read_address(reg).await?;
-        Ok((val & 0b1) == 1)
-    }
-
-    pub async fn get_attached_state(&mut self) -> Result<AttachedState, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::AttachedState.into();
-        let val = self.read_address(reg).await? >> 6;
-        match val & 0b11 {
-            0b00 => Ok(AttachedState::Unattached),
-            0b01 => Ok(AttachedState::Source),
-            0b10 => Ok(AttachedState::Sink),
-            0b11 => Ok(AttachedState::Accessory),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn get_cable_orientation(&mut self) -> Result<CableOrientation, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::CableOrientation.into();
-        let val = self.read_address(reg).await? >> 5;
-        match val & 0b1 {
-            0 => Ok(CableOrientation::Normal),
-            1 => Ok(CableOrientation::Flipped),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn get_interrupt_status(&mut self) -> Result<bool, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::InterruptStatus.into();
-        Ok((self.read_address(reg).await? >> 4) == 1)
-    }
-
-    pub async fn clear_interrupt(&mut self) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::InterruptStatus.into();
-        let mut val = self.read_address(reg).await?;
-        val &= !(0b1 << 4);
-        self.write_address(reg, val).await
-    }
-
-    pub async fn set_drp_duty_cycle(&mut self, duty: DrpDutyCycle) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::DrpDutyCycle.into();
-        let mut val = self.read_address(reg).await?;
-        let mask = !(0b11 << 1);
-        val &= mask | ((duty as u8) << 1);
-        self.write_address(reg, val).await
-    }
-
-    pub async fn get_drp_duty_cycle(&mut self) -> Result<DrpDutyCycle, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::DrpDutyCycle.into();
-        let val = self.read_address(reg).await? >> 1;
-        match val & 0b11 {
-            0b00 => Ok(DrpDutyCycle::Default),
-            0b01 => Ok(DrpDutyCycle::Fast),
-            0b10 => Ok(DrpDutyCycle::Faster),
-            0b11 => Ok(DrpDutyCycle::Fastest),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn get_debounce(&mut self) -> Result<Debounce, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::Debounce.into();
-        let val = self.read_address(reg).await? >> 6;
-        match val & 0b11 {
-            0b00 => Ok(Debounce::D133),
-            0b01 => Ok(Debounce::D116),
-            0b10 => Ok(Debounce::D151),
-            0b11 => Ok(Debounce::D168),
-            _ => unreachable!()
-        }
-     }
-
-    pub async fn set_debounce(&mut self, debounce: Debounce) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::Debounce.into();
-        let mut val = self.read_address(reg).await?;
-        let mask = !(0b11 << 6);
-        val &= mask | ((debounce as u8) << 6);
-        self.write_address(reg, val).await
-    }
-
-    pub async fn get_mode(&mut self) -> Result<Mode, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::ModeSelect.into();
-        let val = self.read_address(reg).await? >> 4;
-        match val & 0b11 {
-            0b00 => Ok(Mode::UsePortPin),
-            0b01 => Ok(Mode::UFP),
-            0b10 => Ok(Mode::DFP),
-            0b11 => Ok(Mode::DRP),
-            _ => unreachable!()
-        }
-    }
-
-    pub async fn set_mode(&mut self, mode: Mode) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::ModeSelect.into();
-        let mut val = self.read_address(reg).await?;
-        let mask = !(0b11 << 4);
-        val &= mask | ((mode as u8) << 4);
-        self.write_address(reg, val).await
-    }
-
-    pub async fn soft_reset(&mut self) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::SoftReset.into();
-        let mut val = self.read_address(reg).await?;
+    pub async fn soft_reset(&mut self) -> Result<(), T::Error> {
+        let mut val = self.read_address(RegisterAddress::Status3).await?;
         val |= 0b1 << 3;
-        self.write_address(reg, val).await
+        self.write_address(RegisterAddress::Status3, val).await
     }
 
-    pub async fn set_rdrp_disabled(&mut self, disable: bool) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::DisableRDRP.into();
-        let mut val = self.read_address(reg).await?;
-        if disable {
-            val |= 0b1;
-        } else {
-            val &= !0b1;
-        }
-        self.write_address(reg, val).await
+    pub fn register<'a, R: RegisterField>(&'a mut self) -> UpdateRegister<'a, T, R> {
+        UpdateRegister::new(R::address(), self)
     }
 
-    pub async fn get_rdrp_disabled(&mut self) -> Result<bool, I2cDeviceError<esp_hal::i2c::master::Error>> {
-        let reg: RegisterAddress = Register::DisableRDRP.into();
-        let val = self.read_address(reg).await? >> 2;
-        Ok((val & 0b1) == 1)
-     }
+    pub fn current_advertisement<'a>(&'a mut self) -> UpdateRegister<'a, T, CurrentAdvertisement> {
+        self.register()
+    }
 
-    pub async fn get_device_id(&mut self) -> Result<[u8;8], I2cDeviceError<esp_hal::i2c::master::Error>> {
+    pub fn current_mode_detect<'a>(&'a mut self) -> UpdateRegister<'a, T, CurrentModeDetect> {
+        self.register()
+    }
+
+    pub fn accessory_connected<'a>(&'a mut self) -> UpdateRegister<'a, T, AccessoryConnected> {
+        self.register()
+    }
+
+    pub fn active_cable_connected<'a>(&'a mut self) -> UpdateRegister<'a, T, ActiveCableConnected> {
+        self.register()
+    }
+
+    pub fn cable_orientation<'a>(&'a mut self) -> UpdateRegister<'a, T, CableOrientation> {
+        self.register()
+    }
+
+    pub fn interrupt_status<'a>(&'a mut self) -> UpdateRegister<'a, T, InterruptStatus> {
+        self.register()
+    }
+
+    pub fn drp_duty_cycle<'a>(&'a mut self) -> UpdateRegister<'a, T, DrpDutyCycle> {
+        self.register()
+    }
+
+    pub fn debounce<'a>(&'a mut self) -> UpdateRegister<'a, T, Debounce> {
+        self.register()
+    }
+
+    pub fn mode_select<'a>(&'a mut self) -> UpdateRegister<'a, T, ModeSelect> {
+        self.register()
+    }
+
+    pub fn disable_rdrp<'a>(&'a mut self) -> UpdateRegister<'a, T, DisableRDRP> {
+        self.register()
+    }
+
+    pub async fn get_device_id(&mut self) -> Result<[u8;8], T::Error> {
         Ok([
             self.read_address(RegisterAddress::DeviceID).await?,
             self.read_address(RegisterAddress::DeviceID1).await?,
@@ -338,19 +277,54 @@ impl TUSB320 {
         ])
     }
 
-    pub async fn read_address(&mut self, reg: RegisterAddress) -> Result<u8, I2cDeviceError<esp_hal::i2c::master::Error>> {
+    async fn read_address(&mut self, reg: RegisterAddress) -> Result<u8, T::Error> {
         let mut response = [0; 1];
-        // 1 means 'read' cycle
-        let i2c_addr = USB_ADDR | 1;
-        self.bus.write_read(i2c_addr, &[reg as u8], &mut response).await?;
+        self.bus.write_read(USB_ADDR_READ, &[reg as u8], &mut response).await?;
         Ok(response[0])
     }
 
-    pub async fn write_address(&mut self, reg: RegisterAddress, value: u8) -> Result<(), I2cDeviceError<esp_hal::i2c::master::Error>> {
-        // 0 means 'write' cycle
-        let i2c_addr = USB_ADDR | 0;
-        self.bus.write(i2c_addr, &[reg as u8]).await?;
-        self.bus.write(i2c_addr, &[value]).await?;
+    async fn write_address(&mut self, reg: RegisterAddress, value: u8) -> Result<(), T::Error> {
+        // FIXME: I think this needs to use self.bus.transaction()
+        self.bus.write(USB_ADDR_WRITE, &[reg as u8]).await?;
+        self.bus.write(USB_ADDR_WRITE, &[value]).await?;
         Ok(())
     }
 }
+
+pub struct UpdateRegister<'a, I2C: AsyncI2c, T> {
+    reg: RegisterAddress,
+    raw: u8,
+    dev: &'a mut TUSB320<I2C>,
+    _type: PhantomData<T>
+}
+
+impl<'a, I2C: AsyncI2c, T> UpdateRegister<'a, I2C, T> {
+    const fn new(reg: RegisterAddress, dev: &'a mut TUSB320<I2C>) -> Self {
+        Self { reg, raw: 0, dev, _type: PhantomData }
+    }
+
+    pub async fn refresh(&mut self) -> Result<(), I2C::Error> {
+        self.raw = self.dev.read_address(self.reg).await?;
+        Ok(())
+    }
+}
+
+impl<'a, I2C: AsyncI2c, T: RegisterField + Into<u8>> UpdateRegister<'a, I2C, T> where u8: Into<T> {
+    pub async fn write(&mut self, value: T) -> Result<(), I2C::Error> {
+        let mask = T::mask();
+        let v: u8 = value.into();
+        self.refresh().await?;
+        self.raw &= !(mask | (v << T::shift()));
+        self.dev.write_address(self.reg, self.raw).await?;
+        Ok(())
+    }
+
+    pub async fn read(&mut self) -> Result<T, I2C::Error> {
+        self.refresh().await?;
+        Ok(self.value())
+    }
+
+    pub fn value(&mut self) -> T {
+        (self.raw >> T::shift()).into()
+    }
+}