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

Cargo.lock

@@ -14,6 +14,15 @@ version = "1.0.99"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b0674a1ddeecb70197781e945de4b3b8ffb61fa939a5597bcf48503737663100"
 
+[[package]]
+name = "approx"
+version = "0.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6"
+dependencies = [
+ "num-traits",
+]
+
 [[package]]
 name = "arrayvec"
 version = "0.7.6"
@@ -756,6 +765,18 @@ dependencies = [
  "esp-metadata-generated",
 ]
 
+[[package]]
+name = "esp-storage"
+version = "0.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f276ad8a3bdc6b47cd92a3e91013f2e42dce9b3fc5023392063387a1ce2ed69a"
+dependencies = [
+ "critical-section",
+ "document-features",
+ "embedded-storage",
+ "esp-rom-sys",
+]
+
 [[package]]
 name = "esp-synopsys-usb-otg"
 version = "0.4.2"
@@ -1052,6 +1073,15 @@ dependencies = [
  "syn",
 ]
 
+[[package]]
+name = "kfilter"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1d320a64bfb4ff8e33add1458241eff87b02a29abf9c19549a02500795928afa"
+dependencies = [
+ "nalgebra",
+]
+
 [[package]]
 name = "libm"
 version = "0.2.15"
@@ -1114,6 +1144,20 @@ dependencies = [
  "libm",
 ]
 
+[[package]]
+name = "nalgebra"
+version = "0.33.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "26aecdf64b707efd1310e3544d709c5c0ac61c13756046aaaba41be5c4f66a3b"
+dependencies = [
+ "approx",
+ "num-complex",
+ "num-rational",
+ "num-traits",
+ "simba",
+ "typenum",
+]
+
 [[package]]
 name = "nb"
 version = "0.1.3"
@@ -1153,6 +1197,15 @@ dependencies = [
  "minimal-lexical",
 ]
 
+[[package]]
+name = "num-complex"
+version = "0.4.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495"
+dependencies = [
+ "num-traits",
+]
+
 [[package]]
 name = "num-derive"
 version = "0.4.2"
@@ -1164,6 +1217,25 @@ dependencies = [
  "syn",
 ]
 
+[[package]]
+name = "num-integer"
+version = "0.1.46"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "num-rational"
+version = "0.4.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824"
+dependencies = [
+ "num-integer",
+ "num-traits",
+]
+
 [[package]]
 name = "num-traits"
 version = "0.2.19"
@@ -1309,10 +1381,13 @@ dependencies = [
  "esp-hal-embassy",
  "esp-hal-smartled",
  "esp-println",
+ "esp-storage",
  "esp-wifi",
  "figments",
+ "kfilter",
  "log",
  "mpu6050-dmp",
+ "nalgebra",
  "nmea",
  "rgb",
  "smart-leds",
@@ -1426,6 +1501,18 @@ dependencies = [
  "unsafe-libyaml",
 ]
 
+[[package]]
+name = "simba"
+version = "0.9.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c99284beb21666094ba2b75bbceda012e610f5479dfcc2d6e2426f53197ffd95"
+dependencies = [
+ "approx",
+ "num-complex",
+ "num-traits",
+ "paste",
+]
+
 [[package]]
 name = "smart-leds"
 version = "0.4.0"

Cargo.toml

@@ -89,7 +89,9 @@ embassy-embedded-hal = "0.5.0"
 embedded-hal-async = "1.0.0"
 smart-leds-trait = "0.3.1"
 anyhow = { version = "1.0.99", default-features = false }
-
+kfilter = "0.4.0"
+nalgebra = { version = "0.33.2", default-features = false }
+esp-storage = { version = "0.7.0", features = ["esp32s3"] }
 
 [profile.dev]
 # Rust debug is too slow.

src/backoff.rs

@@ -56,7 +56,7 @@ impl Backoff {
         'outer: loop {
             for attempt in self.attempts {
                 match &latest {
-                    Err(e) => {
+                    Err(_) => {
                         match attempt {
                             Attempts::Finite(1) => {
                                 warn!("Operation failed on final attempt.");

src/bin/main.rs

@@ -5,27 +5,32 @@
     reason = "mem::forget is generally not safe to do with esp_hal types, especially those \
     holding buffers for the duration of a data transfer."
 )]
+#![feature(future_join)]
 
 use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
 use embassy_executor::Spawner;
-use embassy_sync::blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex};
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 use embassy_sync::mutex::Mutex;
-use embassy_sync::pubsub::PubSubChannel;
 use esp_backtrace as _;
 use esp_hal::i2c::master::{Config, I2c};
+use esp_hal::gpio::Pin;
 use esp_hal::Async;
 use esp_hal::clock::CpuClock;
 use esp_hal::rmt::Rmt;
 use esp_hal::time::Rate;
 use esp_hal::timer::systimer::SystemTimer;
 use esp_hal::timer::timg::TimerGroup;
+use figments::surface::BufferedSurfacePool;
 use log::info;
+use renderbug_embassy::events::BusGarage;
+use renderbug_embassy::tasks::ui::{Ui, ui_main};
+use renderbug_embassy::tasks::gps::gps_task;
+use renderbug_embassy::tasks::motion::motion_task;
 use static_cell::StaticCell;
 
 use renderbug_embassy::{
-    tasks::i2c::*,
-    tasks::render::render,
-    events::Notification
+    tasks::mpu::*,
+    tasks::render::render
 };
 
 extern crate alloc;
@@ -35,7 +40,8 @@ extern crate alloc;
 esp_bootloader_esp_idf::esp_app_desc!();
 
 static I2C_BUS: StaticCell<Mutex<CriticalSectionRawMutex, I2c<'static, Async>>> = StaticCell::new();
-static EVENT_BUS: StaticCell<PubSubChannel<NoopRawMutex, Notification, 4, 4, 4>> = StaticCell::new();
+static BUS_GARAGE: StaticCell<BusGarage> = StaticCell::new();
+
 #[esp_hal_embassy::main]
 async fn main(spawner: Spawner) {
     esp_println::logger::init_logger_from_env();
@@ -46,12 +52,12 @@ async fn main(spawner: Spawner) {
     esp_alloc::heap_allocator!(size: 64 * 1024);
 
     let timer0 = SystemTimer::new(peripherals.SYSTIMER);
-    esp_hal_embassy::init(timer0.alarm0);
-
+    esp_hal_embassy::init([timer0.alarm0, timer0.alarm1, timer0.alarm2]);
     info!("Embassy initialized!");
 
-    let events = EVENT_BUS.init(PubSubChannel::<NoopRawMutex, Notification, 4, 4, 4>::new());
+    let garage = BUS_GARAGE.init(Default::default());
 
+    info!("Setting up wifi");
     let rng = esp_hal::rng::Rng::new(peripherals.RNG);
     let timer1 = TimerGroup::new(peripherals.TIMG0);
     let wifi_init =
@@ -59,17 +65,23 @@ async fn main(spawner: Spawner) {
     let (mut _wifi_controller, _interfaces) = esp_wifi::wifi::new(&wifi_init, peripherals.WIFI)
         .expect("Failed to initialize WIFI controller");
 
+    info!("Launching i2c sensor tasks");
+    let i2c = I2c::new(peripherals.I2C1, Config::default().with_frequency(Rate::from_khz(400)).with_timeout(esp_hal::i2c::master::BusTimeout::Maximum)).unwrap().with_scl(peripherals.GPIO4).with_sda(peripherals.GPIO3).into_async();
+    let i2c_bus = I2C_BUS.init(Mutex::new(i2c));
+
+    spawner.must_spawn(mpu_task(garage.motion.sender(), garage.scenes.dyn_sender(), I2cDevice::new(i2c_bus)));
+    spawner.must_spawn(gps_task(garage.motion.sender(), garage.scenes.dyn_sender(), I2cDevice::new(i2c_bus)));
+
+    info!("Launching motion engine");
+    spawner.must_spawn(motion_task(garage.motion.receiver(), garage.scenes.dyn_sender()));
+
+    info!("Setting up rendering pipeline");
+    let mut surfaces = BufferedSurfacePool::default();
+    let ui = Ui::new(&mut surfaces, &garage.display);
     let frequency: Rate = Rate::from_mhz(80);
     let rmt = Rmt::new(peripherals.RMT, frequency)
         .expect("Failed to initialize RMT").into_async();
     let rmt_channel = rmt.channel0;
-    info!("Launching render task");
-    spawner.must_spawn(render(events.subscriber().unwrap(), rmt_channel, peripherals.GPIO5));
-
-    info!("Launching i2c task");
-    let i2c = I2c::new(peripherals.I2C1, Config::default().with_frequency(Rate::from_khz(400))).unwrap().with_scl(peripherals.GPIO36).with_sda(peripherals.GPIO33).into_async();
-    let i2c_bus = I2C_BUS.init(Mutex::new(i2c));
-
-    spawner.spawn(mpu_task(events.dyn_publisher().unwrap(), I2cDevice::new(i2c_bus))).unwrap();
-    spawner.spawn(gps_task(events.dyn_publisher().unwrap(), I2cDevice::new(i2c_bus))).unwrap();
+    spawner.must_spawn(render(rmt_channel, peripherals.GPIO5.degrade(), surfaces, &garage.display));
+    spawner.must_spawn(ui_main(garage.scenes.dyn_receiver(), ui));
 }

src/ego/alignment.rs

@@ -0,0 +1,56 @@
+use log::*;
+use nalgebra::{Matrix3, Rotation3, Unit, Vector3};
+
+type Sample = Vector3<f32>; // [ax, ay, az] in m/s^2
+
+#[derive(Debug, Clone, Copy)]
+pub struct SensorAlignment {
+    /// 3x3 transform matrix: raw -> body
+    /// includes permutation, sign, and bias subtraction.
+    pub transform: Rotation3<f32>,
+    /// bias vector in body frame
+    pub accel_bias: Vector3<f32>,
+
+    pub gyro_bias: Vector3<f32>,
+}
+
+impl SensorAlignment {
+    pub fn apply(&self, raw: &Sample) -> Vector3<f32> {
+        self.transform * raw - self.accel_bias
+    }
+
+    pub fn apply_gyro(&self, raw: &Sample) -> Vector3<f32> {
+        self.transform * raw - self.gyro_bias
+    }
+
+    pub fn new(accel_samples: &[Sample], gyro_samples: &[Sample]) -> SensorAlignment {
+        let avg: Vector3<f32> = accel_samples.iter().sum::<Vector3<f32>>() / (accel_samples.len() as f32);
+        let down_body = Unit::new_normalize(avg);
+        let down_world = Unit::new_normalize(Vector3::new(0.0, 0.0, -1.0));
+        let rot_down = Rotation3::rotation_between(&down_world, &down_body).unwrap_or_else(Rotation3::identity);
+        let forward_world = Vector3::x();
+
+        let forward_projected = (forward_world - forward_world.dot(&down_world) * down_world.into_inner()).normalize();
+
+        // Transform forward into the body frame
+        let forward_body = rot_down * forward_projected;
+
+        // Construct a proper body frame basis
+        let right_body = down_body.cross(&forward_body).normalize();
+        let forward_body = right_body.cross(&down_body).normalize();
+
+        // Construct rotation matrix from basis vectors
+        let rot = Matrix3::from_columns(&[forward_body, right_body, down_body.into_inner()]);
+        let transform = Rotation3::from_matrix_unchecked(rot);
+
+        let gyro_avg: Vector3<f32> = gyro_samples.iter().sum::<Vector3<f32>>() / (gyro_samples.len() as f32);
+
+        info!("down_body={down_body:?} down_world={down_world:?}");
+
+        Self {
+            transform,
+            accel_bias: transform * avg,
+            gyro_bias: transform * gyro_avg
+        }
+    }
+}

src/ego/heading.rs

@@ -0,0 +1,37 @@
+use core::f32::consts::PI;
+
+/// Normalize angle to [-PI, PI]
+fn wrap_angle(angle: f32) -> f32 {
+    let mut a = angle;
+    while a >  PI { a -= 2.0*PI; }
+    while a < -PI { a += 2.0*PI; }
+    a
+}
+
+/// Heading estimator using gyro yaw rate
+#[derive(Debug, Clone, Copy)]
+pub struct HeadingEstimator {
+    heading: f32, // radians
+}
+
+impl HeadingEstimator {
+    pub fn new(initial_heading: f32) -> Self {
+        Self { heading: wrap_angle(initial_heading) }
+    }
+
+    /// Integrate yaw rate (rad/s) over dt (s)
+    pub fn update(&mut self, yaw_rate: f32, dt: f32) {
+        self.heading = wrap_angle(self.heading + yaw_rate * dt);
+    }
+
+    /// Correct drift using an external absolute heading (e.g. magnetometer)
+    pub fn correct(&mut self, measured_heading: f32, alpha: f32) {
+        // alpha ∈ [0,1]: 0 = ignore measurement, 1 = full trust measurement
+        let err = wrap_angle(measured_heading - self.heading);
+        self.heading = wrap_angle(self.heading + alpha * err);
+    }
+
+    pub fn heading(&self) -> f32 {
+        self.heading
+    }
+}

src/ego/mod.rs

@@ -0,0 +1,3 @@
+pub mod alignment;
+pub mod tilt;
+pub mod heading;

src/ego/tilt.rs

@@ -0,0 +1,89 @@
+use core::f32::consts::PI;
+use nalgebra::ComplexField;
+use nalgebra::RealField;
+
+const G: f32 = 9.80665;
+
+/// Complementary filter state
+pub struct TiltEstimator {
+    roll: f32,   // phi
+    pitch: f32,  // theta
+    yaw: f32,    // psi (integrated gyro yaw, will drift)
+    alpha: f32,  // complementary weight for gyro integration (0..1)
+}
+
+impl TiltEstimator {
+    pub fn new(alpha: f32) -> Self {
+        Self { roll: 0.0, pitch: 0.0, yaw: 0.0, alpha }
+    }
+
+    /// dt in seconds, accel in m/s^2 body frame, gyro in rad/s body rates (gx, gy, gz)
+    /// body axes assumed: x forward, y right, z down
+    pub fn update(&mut self, dt: f32, accel: (f32, f32, f32), gyro: (f32, f32, f32)) {
+        let (ax, ay, az) = accel;
+        let (gx, gy, gz) = gyro;
+
+        // 1) accel-based tilt estimates (avoid NaN by small epsilon)
+        let eps = 1e-6;
+        let roll_acc = (ay).atan2(az.max(eps)); // phi = atan2(ay, az)
+        let pitch_acc = (-ax).atan2((ay*ay + az*az).sqrt().max(eps)); // theta = atan2(-ax, sqrt(ay^2+az^2))
+
+        // 2) integrate gyro for angles (body rates -> approx angle rates)
+        // Note: for small angles, roll_dot ≈ gx, pitch_dot ≈ gy, yaw_dot ≈ gz in body frame if axes aligned.
+        // For better accuracy you'd convert rates to inertial derivatives but this is common and simple.
+        let roll_from_gyro = self.roll + gx * dt;
+        let pitch_from_gyro = self.pitch + gy * dt;
+        let yaw_from_gyro = self.yaw + gz * dt;
+
+        // 3) complementary combine
+        let alpha = self.alpha;
+        self.roll  = alpha * roll_from_gyro  + (1.0 - alpha) * roll_acc;
+        self.pitch = alpha * pitch_from_gyro + (1.0 - alpha) * pitch_acc;
+        self.yaw   = yaw_from_gyro; // we typically don't correct yaw with accel (no observability)
+
+        // normalize yaw to -pi..pi
+        if self.yaw > PI { self.yaw -= 2.0*PI; }
+        if self.yaw < -PI { self.yaw += 2.0*PI; }
+    }
+
+    pub fn angles(&self) -> (f32, f32, f32) {
+        (self.roll, self.pitch, self.yaw)
+    }
+}
+
+/// Rotate body accel into ENU and remove gravity; returns (east, north, up) linear accel in m/s^2
+pub fn accel_body_to_enu_lin(body_acc: (f32,f32,f32), roll: f32, pitch: f32, yaw: f32) -> (f32,f32,f32) {
+    let (ax, ay, az) = body_acc;
+
+    let (sr, cr) = roll.sin_cos();
+    let (sp, cp) = pitch.sin_cos();
+    let (sy, cy) = yaw.sin_cos();
+
+    // Build R = Rz(yaw) * Ry(pitch) * Rx(roll)
+    // multiply R * a_body
+    let r11 =  cy*cp;
+    let r12 =  cy*sp*sr - sy*cr;
+    let r13 =  cy*sp*cr + sy*sr;
+
+    let r21 =  sy*cp;
+    let r22 =  sy*sp*sr + cy*cr;
+    let r23 =  sy*sp*cr - cy*sr;
+
+    let r31 = -sp;
+    let r32 =  cp*sr;
+    let r33 =  cp*cr;
+
+    // a_nav = R * a_body
+    let ax_nav = r11*ax + r12*ay + r13*az; // east
+    let ay_nav = r21*ax + r22*ay + r23*az; // north
+    let az_nav = r31*ax + r32*ay + r33*az; // up
+
+    // remove gravity (nav frame gravity points down in NED; in our ENU 'up' axis positive up)
+    // If accel measured gravity as +9.8 on sensor when static and z down convention used above,
+    // then az_nav will include +g in 'up' direction; subtract G.
+    let ax_lin = ax_nav;
+    let ay_lin = ay_nav;
+    let az_lin = az_nav - G; // linear acceleration in up axis
+
+    (ax_lin, ay_lin, az_lin)
+}

src/events.rs

@@ -1,19 +1,68 @@
 
-#[derive(Clone, Copy)]
+use core::sync::atomic::{AtomicBool, AtomicU8};
+
+use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::Channel};
+use nalgebra::{Vector2, Vector3};
+
+#[derive(Clone, Copy, Default, Debug)]
 pub enum Scene {
-    ParkedIdle, // Default state when booting up or waking up from long term (overnight, eg) parking, or entered when accelerometers and GPS both show zero motion for ~30 seconds
+    #[default]
+    Startup, // Default state when booting up
+    ParkedIdle, // Default state when waking up from long term (overnight, eg) parking, or entered when accelerometers and GPS both show zero motion for ~30 seconds
     StoplightIdle, // Entered when GPS speed is zero after decelerating
     Accelerating, // GPS speed is increasing
     Decelerating, // GPS speed is decreasing
     ParkedLongTerm, // GPS has not changed location in the last ~5 minutes
 }
 
-#[derive(Clone, Copy)]
+#[derive(Clone, Copy, Debug)]
+pub enum Measurement {
+    // GPS coordinates
+    GPS(Vector2<f64>),
+    // Accelerometer values in body frame where x=forwards
+    IMU(Vector3<f32>)
+}
+
+#[derive(Clone, Copy, Debug)]
 pub enum Notification {
     SceneChange(Scene),
     WifiConnected,
     WifiDisconnected,
     GPSLost,
     GPSAcquired,
-    BPMBeat
+    CalibrationComplete,
+    SetHeadlight(bool),
+    SetBrakelight(bool)
+}
+
+#[derive(Debug)]
+pub struct DisplayControls {
+    pub on: AtomicBool,
+    pub brightness: AtomicU8
+}
+
+impl Default for DisplayControls {
+    fn default() -> Self {
+        Self {
+            on: AtomicBool::new(true),
+            brightness: AtomicU8::new(255)
+        }
+    }
+}
+
+#[derive(Debug)]
+pub struct BusGarage {
+    pub motion: Channel<NoopRawMutex, Measurement, 4>,
+    pub scenes: Channel<CriticalSectionRawMutex, Notification, 4>,
+    pub display: DisplayControls
+}
+
+impl Default for BusGarage {
+    fn default() -> Self {
+        Self {
+            motion: Channel::new(),
+            scenes: Channel::new(),
+            display: Default::default()
+        }
+    }
 }

src/lib.rs

@@ -4,6 +4,8 @@ pub mod display;
 pub mod backoff;
 pub mod events;
 pub mod tasks;
+pub mod shaders;
+pub mod ego;
 extern crate alloc;
 
 use rgb::Rgb;

src/shaders.rs

@@ -0,0 +1,125 @@
+use core::cmp::max;
+
+use figments::{liber8tion::{interpolate::{ease_in_out_quad}, noise::inoise8, trig::{cos8, sin8}}, prelude::*};
+use rgb::Rgba;
+
+use crate::{display::BikeSpace, tasks::render::Uniforms};
+
+#[derive(Clone, Copy, Default)]
+pub struct Background {
+    color: Option<Rgb<u8>>
+}
+
+impl Background {
+    pub fn from_color(color: Rgb<u8>) -> Self {
+        Self {
+            color: Some(color)
+        }
+    }
+}
+
+impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Background {
+    fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
+        let noise_x = sin8(uniforms.frame % 255) as i16;
+        let noise_y = cos8(uniforms.frame % 255) as i16;
+        let brightness = inoise8(noise_x.wrapping_add(coords.x as i16), noise_y.wrapping_add(coords.y as i16));
+        let saturation = inoise8(noise_y.wrapping_add(coords.y as i16), noise_x.wrapping_add(coords.x as i16));
+        let rgb: Rgb<u8> = match self.color {
+            None => Hsv::new(uniforms.primary_color.hue, max(128, saturation), brightness).into(),
+            Some(c) => c
+        };
+
+        Rgba::new(rgb.r, rgb.g, rgb.b, 255)
+    }
+}
+
+#[derive(Clone, Copy, Default)]
+pub struct Tail {}
+impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Tail {
+    fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
+        let hue_offset = 0u8.lerp8by8(16, sin8(uniforms.frame.wrapping_sub(coords.x))) as i8 - 8;
+        let value = max(30, inoise8(coords.x.wrapping_add(uniforms.frame) as i16, coords.y.wrapping_add(uniforms.frame) as i16));
+        Hsv::new(hue_offset.wrapping_sub(uniforms.primary_color.hue as i8) as u8, max(210, sin8(uniforms.frame.wrapping_add(coords.x))), value).into()
+    }
+}
+
+#[derive(Clone, Copy, Default)]
+pub struct Brakelight {}
+
+impl Brakelight {
+    pub const fn end() -> usize {
+        87
+    }
+
+    pub const fn length() -> usize {
+        50
+    }
+
+    pub const fn safety_length() -> usize {
+        15
+    }
+
+    pub const fn rectangle() -> Rectangle<BikeSpace> {
+        Rectangle::new_from_coordinates(Self::end() - Self::length(), 5, Self::end(), 5)
+    }
+
+}
+
+impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Brakelight {
+
+    fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
+        let distance_from_end = Self::end() - coords.x;
+        
+        if distance_from_end < Self::safety_length() {
+            Rgba::new(max(128, sin8(uniforms.frame.wrapping_sub(coords.x))), 0, 0, 255)
+        } else {
+            let pct = (distance_from_end as f32 / Self::length() as f32) * 255f32;
+            Rgba::new(max(100, ease_in_out_quad((pct as u8).wrapping_add(uniforms.frame as u8))), 0, 0, ease_in_out_quad(255 - pct as u8))
+        }
+    }
+}
+
+#[derive(Clone, Copy, Default)]
+pub struct Headlight {}
+impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Headlight {
+    fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
+        Hsv::new(0, 0, max(100, ease_in_out_quad(sin8(uniforms.frame.wrapping_sub(coords.x))))).into()
+    }
+}
+
+#[derive(Clone, Copy, Default)]
+pub struct Panel {}
+impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Panel {
+    fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
+        let noise_offset = max(180, inoise8(coords.x.wrapping_add(uniforms.frame) as i16, coords.y.wrapping_add(uniforms.frame) as i16));
+        let pct = (coords.x as f32 / 18f32) * 255f32;
+        let shift = match coords.y {
+            1..=2 => 106, // 150 degrees
+            3..=4 => 148, // 210 degrees
+            _ => 0
+        };
+        Hsv::new(uniforms.primary_color.hue.wrapping_add(shift), noise_offset, max(100, sin8(pct as u8).wrapping_add(uniforms.frame as u8))).into()
+    }
+}
+
+#[derive(Clone, Copy, Default)]
+pub struct Thinking {}
+
+impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Thinking {
+    fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
+        //let noise_x = sin8(sin8((frame % 255) as u8).wrapping_add(coords.x));
+        //let noise_y = cos8(cos8((frame % 255) as u8).wrapping_add(coords.y));
+        let offset_x = sin8(uniforms.frame.wrapping_add(coords.x));
+        let offset_y = cos8(uniforms.frame.wrapping_add(coords.y));
+        let noise_x = offset_x / 2;
+        let noise_y = offset_y / 2;
+        //let noise_x = coords.x.wrapping_add(offset_x);
+        //let noise_y = coords.y.wrapping_add(offset_y);
+        Hsv::new(
+            inoise8(offset_x as i16, offset_y as i16),
+            128_u8.saturating_add(inoise8(noise_y as i16, noise_x as i16)),
+            255
+        ).into()
+    }
+}
+                

src/tasks/gps.rs

@@ -0,0 +1,96 @@
+use alloc::string::String;
+use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
+use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::{DynamicSender, Sender}};
+use embassy_time::Timer;
+use embedded_hal_async::i2c::I2c as _;
+use esp_hal::{i2c::master::I2c, Async};
+use log::*;
+use nalgebra::Vector2;
+use nmea::Nmea;
+
+use crate::{backoff::Backoff, events::{Measurement, Notification}};
+
+#[allow(dead_code)] //FIXME: Allow switching to this via configure option
+const GPS_TEST_DATA: &str = include_str!("../test.nmea");
+
+#[embassy_executor::task]
+pub async fn gps_task(events: Sender<'static, NoopRawMutex, Measurement, 4>, sink: DynamicSender<'static, Notification>, mut i2c_bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
+    Backoff::from_secs(5).forever().attempt::<_, (), ()>(async || {
+        Backoff::from_secs(5).forever().attempt(async || {
+            info!("Initializing GPS");
+            // Enable a bunch of data? idk
+            let bytes = "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
+            i2c_bus.write(0x10, bytes.as_bytes()).await?;
+
+            // 1hz updates
+            let bytes = "$PMTK220,1000*1F\r\n";
+            i2c_bus.write(0x10, bytes.as_bytes()).await?;
+
+            // 1hz position fix
+            let bytes = "$PMTK300,1000,0,0,0,0*1C\r\n";
+            i2c_bus.write(0x10, bytes.as_bytes()).await?;
+
+            // Antenna updates
+            let bytes = "$PGCMD,33,1*6C\r\n";
+            i2c_bus.write(0x10, bytes.as_bytes()).await
+        }).await.unwrap();
+
+        let mut strbuf = String::new();
+
+        let mut parser = Nmea::default();
+        let mut parsing = false;
+        let mut has_lock = false;
+        //let mut iter = GPS_TEST_DATA.as_bytes().iter();
+        info!("GPS is ready!");
+        loop {
+            let mut buf = [0; 1];
+            i2c_bus.read(0x10, &mut buf).await.map_err(|_| { Err::<(), ()>(()) }).ok();
+            //buf[0] = *(iter.next().unwrap());
+            if (buf[0] as char == '\n' || buf[0] as char == '\r') && !strbuf.is_empty() {
+                if let Ok(result) = parser.parse(&strbuf) {
+                    match parser.fix_type {
+                        None if has_lock => {
+                            sink.send(Notification::GPSLost).await;
+                            has_lock = false
+                        },
+                        None => (),
+                        Some(_) => {
+                            if !has_lock {
+                                sink.send(Notification::GPSAcquired).await;
+                                has_lock = true;
+                            }
+
+                            //TODO: 4 satellites seems to be "Some" fix, 6 is a perfect fix
+                            //TODO: Only send updates when we get the correct nmea sentence
+                            if let (Some(lat), Some(lng)) = (parser.latitude, parser.longitude) {
+                                events.send(Measurement::GPS(Vector2::new(lat, lng))).await;
+                            }
+                        }
+                    }
+                    log::info!("nmea={result:?} raw={strbuf:?}");
+                    log::debug!("nmea={parser:?}");
+                    log::info!("speed={:?} altitude={:?} lat={:?} lng={:?} fix={:?}", parser.speed_over_ground, parser.altitude, parser.latitude, parser.longitude, parser.fix_type);
+                    for sat in parser.satellites() {
+                        info!("\t{} snr={:?} prn={:?}", sat.gnss_type(), sat.snr(), sat.prn())
+                    }
+                } else {
+                    log::warn!("Unhandled NMEA {strbuf:?}");
+                }
+                strbuf = String::new();
+                parsing = false;
+                // Update frequency is 1hz, so we should never get an update faster than once per second
+                Timer::after_secs(1).await;
+            } else if strbuf.is_empty() && (buf[0] as char == '$' || buf[0] as char == '!') {
+                parsing = true;
+                strbuf.push(buf[0] as char);
+                Timer::after_millis(10).await;
+            } else if parsing {
+                strbuf.push(buf[0] as char);
+                Timer::after_millis(10).await;
+            } else {
+                // If there is no data ready for some reason, wait 500ms, which should place us at least somewhere after the next data frame is ready to read.
+                Timer::after_millis(500).await;
+            }
+        }
+    }).await.ok();
+}

src/tasks/i2c.rs

@@ -1,136 +0,0 @@
-use core::cell::RefCell;
-
-use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
-use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, pubsub::DynPublisher};
-use embassy_time::{Delay, Timer};
-use embedded_hal_async::i2c::I2c as _;
-use esp_hal::{i2c::master::I2c, Async};
-use log::{info, warn};
-use mpu6050_dmp::{address::Address, error_async::Error, sensor_async::Mpu6050};
-use nmea::Nmea;
-use alloc::string::String;
-
-use crate::{backoff::Backoff, events::Notification};
-
-#[embassy_executor::task]
-pub async fn mpu_task(events: DynPublisher<'static, Notification>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
-    let backoff = Backoff::from_millis(5);
-    let busref = RefCell::new(Some(bus));
-
-    backoff.forever().attempt::<_, (), ()>(async || {
-        let mut sensor = backoff.attempt(async || {
-            info!("Initializing MPU");
-            match Mpu6050::new(busref.replace(None).unwrap(), Address::default()).await.map_err(|e| { e.i2c }) {
-                Err(i2c) => {
-                    busref.replace(Some(i2c));
-                    Err(())
-                },
-                Ok(mut sensor) => {
-                    match backoff.attempt(async || { mpu_init(&mut sensor).await }).await {
-                        Err(_) => {
-                            busref.replace(Some(sensor.release()));
-                            Err(())
-                        },
-                        Ok(_) => Ok(sensor)
-                    }
-                }
-            }
-        }).await?;
-        info!("MPU is ready!");
-        let sensor_ref = &mut sensor;
-        loop {
-            match backoff.attempt(async || { sensor_ref.motion6().await }).await {
-                Ok((accel_data, gyro_data)) => {
-                    info!("Accel x={} y={} z={}", accel_data.x() as i32, accel_data.y() as i32, accel_data.z() as i32);
-                    info!("Gyro x={} y={} z={}", gyro_data.x() as i32, gyro_data.y() as i32, gyro_data.z() as i32);
-                    Timer::after_millis(5000).await;
-                },
-                Err(e) => {
-                    warn!("Failed to read MPU motion data! {e:?}");
-                    busref.replace(Some(sensor.release()));
-                    return Err(());
-                }
-            };
-        }
-    }).await.unwrap();
-}
-
-async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>) -> Result<(), Error<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>> {
-    let mut delay = Delay;
-    let backoff = Backoff::from_millis(3);
-    info!("Resetting MPU");
-    backoff.attempt(async || {sensor.reset(&mut delay).await}).await?;
-    info!("Configuring sample rate");
-    backoff.attempt(async || {sensor.set_sample_rate_divider(255).await}).await
-}
-
-const GPS_TEST_DATA: &str = include_str!("../test.nmea");
-
-#[embassy_executor::task]
-pub async fn gps_task(events: DynPublisher<'static, Notification>, mut i2c_bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
-    Backoff::from_secs(5).forever().attempt(async || {
-        info!("Initializing GPS");
-        // Enable a bunch of data? idk
-        let bytes = "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
-        i2c_bus.write(0x10, bytes.as_bytes()).await?;
-
-        // 1hz updates
-        let bytes = "$PMTK220,1000*1F\r\n";
-        i2c_bus.write(0x10, bytes.as_bytes()).await?;
-
-        // 1hz position fix
-        let bytes = "$PMTK300,1000,0,0,0,0*1C\r\n";
-        i2c_bus.write(0x10, bytes.as_bytes()).await?;
-
-        // Antenna updates
-        let bytes = "$PGCMD,33,1*6C\r\n";
-        i2c_bus.write(0x10, bytes.as_bytes()).await
-    }).await.unwrap();
-
-    let mut strbuf = String::new();
-
-    let mut parser = Nmea::default();
-    let mut parsing = false;
-    let mut has_lock = false;
-    //let mut iter = GPS_TEST_DATA.as_bytes().iter();
-    info!("GPS is ready!");
-    loop {
-        let mut buf = [0; 1];
-        i2c_bus.read(0x10, &mut buf).await.unwrap();
-        //buf[0] = *(iter.next().unwrap());
-        if (buf[0] as char == '\n' || buf[0] as char == '\r') && !strbuf.is_empty() {
-            if let Ok(result) = parser.parse(&strbuf) {
-                if parser.fix_type.is_some() != has_lock {
-                    has_lock = parser.fix_type.is_some();
-                    if has_lock {
-                        events.publish(Notification::GPSAcquired).await;
-                    } else {
-                        events.publish(Notification::GPSLost).await;
-                    }
-                }
-                log::info!("nmea={result:?} raw={strbuf:?}");
-                log::debug!("nmea={parser:?}");
-                log::info!("speed={:?} altitude={:?} lat={:?} lng={:?} fix={:?}", parser.speed_over_ground, parser.altitude, parser.latitude, parser.longitude, parser.fix_type);
-                for sat in parser.satellites() {
-                    info!("\t{} snr={:?} prn={:?}", sat.gnss_type(), sat.snr(), sat.prn())
-                }
-            } else {
-                log::warn!("Unhandled NMEA {strbuf:?}");
-            }
-            strbuf = String::new();
-            parsing = false;
-            // Update frequency is 1hz, so we should never get an update faster than once per second
-            Timer::after_secs(1).await;
-        } else if strbuf.is_empty() && (buf[0] as char == '$' || buf[0] as char == '!') {
-            parsing = true;
-            strbuf.push(buf[0] as char);
-            Timer::after_millis(3).await;
-        } else if parsing {
-            strbuf.push(buf[0] as char);
-            Timer::after_millis(3).await;
-        } else {
-            // If there is no data ready for some reason, wait 500ms, which should place us at least somewhere after the next data frame is ready to read.
-            Timer::after_millis(500).await;
-        }
-    }
-}

src/tasks/mod.rs

@@ -1,2 +1,5 @@
-pub mod i2c;
+pub mod mpu;
 pub mod render;
+pub mod motion;
+pub mod gps;
+pub mod ui;

src/tasks/motion.rs

@@ -0,0 +1,159 @@
+#![allow(non_snake_case)] // WIP, because most of this code came from chatgpt and I'm still not sure if it actually works or is completely wrong
+use embassy_sync::{blocking_mutex::raw::NoopRawMutex, channel::{DynamicSender, Receiver}};
+use nalgebra::{SMatrix, SVector};
+
+use crate::{ego::heading::HeadingEstimator, events::{Measurement, Notification}};
+use log::*;
+use nalgebra::ComplexField;
+
+/// State: [px, py, vx, vy]
+type State = SVector<f32, 6>;
+type Control = SVector<f32, 3>; // dx, dx, yaw
+type GpsMeasurement = SVector<f32, 2>; // gps x, gps y
+
+pub struct Kalman {
+    x: State,                          // state estimate
+    P: SMatrix<f32, 6, 6>,             // covariance
+    F: SMatrix<f32, 6, 6>,             // state transition
+    B: SMatrix<f32, 6, 3>,             // control matrix
+    H: SMatrix<f32, 2, 6>,             // measurement matrix
+    Q: SMatrix<f32, 6, 6>,             // process noise
+    R: SMatrix<f32, 2, 2>,             // measurement noise
+}
+
+impl Kalman {
+    pub fn new(dt: f32) -> Self {
+        let mut kf = Kalman {
+            x: State::zeros(),
+            P: SMatrix::identity(),
+            F: SMatrix::identity(),
+            B: SMatrix::zeros(),
+            H: SMatrix::zeros(),
+            Q: SMatrix::identity() * 5.0,
+            R: SMatrix::identity() * 5.0, // GPS noise
+        };
+
+        // F (state transition)
+        kf.F[(0,2)] = dt;
+        kf.F[(1,3)] = dt;
+        kf.F[(4,5)] = dt;
+
+        // B (control matrix)
+        kf.B[(0,0)] = 0.5 * dt * dt;
+        kf.B[(1,1)] = 0.5 * dt * dt;
+        kf.B[(2,0)] = dt;
+        kf.B[(3,1)] = dt;
+        kf.B[(5,2)] = dt;
+
+        // H (GPS: positions only)
+        kf.H[(0,0)] = 1.0;
+        kf.H[(1,1)] = 1.0;
+
+        kf
+    }
+
+    pub fn predict(&mut self, u: &Control) {
+        self.x = self.F * self.x + self.B * u;
+        self.P = self.F * self.P * self.F.transpose() + self.Q;
+    }
+
+    pub fn update(&mut self, z: &GpsMeasurement) {
+        let y = z - self.H * self.x; // innovation
+        let S = self.H * self.P * self.H.transpose() + self.R;
+        if let Some(s_inv) = S.try_inverse() {
+            let K = self.P * self.H.transpose() * s_inv;
+            self.x += K * y;
+            self.P = (SMatrix::<f32,6,6>::identity() - K * self.H) * self.P;
+        }
+    }
+
+    // Used to indicate stationary behavior and therefore velocities should be zero
+    pub fn update_zupt(&mut self) {
+        let z = SVector::<f32, 2>::zeros(); // velocity = 0
+        let mut H = SMatrix::<f32, 2, 6>::zeros();
+        H[(0,2)] = 1.0; // vx
+        H[(1,3)] = 1.0; // vy
+
+        let R = SMatrix::<f32, 2, 2>::identity() * 0.01; // very confident
+
+        let y = z - H * self.x;
+        let S = H * self.P * H.transpose() + R;
+
+        if let Some(s_inv) = S.try_inverse() {
+            let k = self.P * H.transpose() * s_inv;
+            self.x += k * y;
+            self.P = (SMatrix::<f32,6,6>::identity() - k * H) * self.P;
+        }
+    }
+
+    pub fn state(&self) -> &State {
+        &self.x
+    }
+}
+
+#[embassy_executor::task]
+pub async fn motion_task(src: Receiver<'static, NoopRawMutex, Measurement, 4>, sink: DynamicSender<'static, Notification>) {
+    //TODO: is this the right value? how do update frequencies work?
+    let dt = 0.01; // 10 Hz
+    let mut kf = Kalman::new(dt);
+    let mut is_moving = false;
+    let mut heading = HeadingEstimator::new(0.0);
+
+    //TODO: Accelerating/decelerating events need thresholds, maybe a PID to detect? PID setpoint can be a running average speed, and if the calculated error is above/below a threshold, it means we are accelerating/decelerating
+
+    let mut last_fix = None;
+
+    loop {
+        match src.receive().await {
+            Measurement::IMU(imu_accel) => {
+                //info!("IMU update {imu_accel:?}");
+                //info!("m=\t({:?}, {:?})\tyaw={:?}", imu_accel.x, imu_accel.y, imu_accel.z);
+                //kf.predict(&Control::new(imu_accel.x, imu_accel.y, imu_accel.z));
+                // TODO: Also needs corrected using GPS headings via rotation from body frame into world frame
+                //heading.heading();
+                heading.update(imu_accel.z, 0.01);
+
+                //info!("{}", imu_accel.xy().magnitude());
+                if imu_accel.xy().magnitude() >= 0.2 {
+                    kf.predict(&imu_accel);
+                } else {
+                    kf.update_zupt();
+                }
+                // TODO: Rotate the acceleration values into the World frame using GPS heading values
+
+                // X means forwards, Y is left/right slide, Z is vertical movement
+                if imu_accel.x > 1.0 {
+                    sink.send(Notification::SceneChange(crate::events::Scene::Accelerating)).await;
+                } else if imu_accel.x < -1.0 {
+                    sink.send(Notification::SceneChange(crate::events::Scene::Decelerating)).await;
+                }
+            }
+            Measurement::GPS(gps_pos) => {
+                info!("GPS update! {gps_pos:?}");
+                match last_fix {
+                    None => last_fix = Some(gps_pos),
+                    Some(coords) => {
+                        let dx = (gps_pos.x - coords.x) * coords.x.cos() * 6371000.0;
+                        let dy = (gps_pos.y - coords.y) * 6371000.0;
+                        kf.update(&GpsMeasurement::new(dx as f32, dy as f32));
+                        //last_fix = Some(gps_pos);
+                    }
+                }
+            }
+        }
+
+        let est = kf.state();
+        if est[2].abs() + est[3].abs() >= 0.2 {
+            info!("pos=\t({},\t{})\tvel=({},\t{})\theading=({},\t{})", est[0], est[1], est[2], est[3], est[4], est[5]);
+        }
+
+        if est[2].abs() + est[3].abs() > 0f32 {
+            if !is_moving {
+                is_moving = true;
+            }
+        } else if is_moving {
+            is_moving = false;
+            sink.send(Notification::SceneChange(crate::events::Scene::StoplightIdle)).await;
+        }
+    }
+}

src/tasks/mpu.rs

@@ -0,0 +1,180 @@
+use core::cell::RefCell;
+
+use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
+use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::{DynamicSender, Sender}};
+use embassy_time::{Delay, Timer};
+use esp_hal::{i2c::master::I2c, Async};
+use log::{info, warn};
+use mpu6050_dmp::{address::Address, calibration::CalibrationParameters, error_async::Error, sensor_async::Mpu6050};
+use nalgebra::Vector3;
+use core::f32::consts::PI;
+use nalgebra::ComplexField;
+
+use crate::{backoff::Backoff, ego::{alignment::SensorAlignment, tilt::TiltEstimator}, events::{Measurement, Notification}};
+
+const G: f32 = 9.80665;
+const SENS_2G: f32 = 16384.0;
+const DEG2RAD: f32 = PI / 180.0;
+
+/// Convert raw accel (i16) to m/s^2
+fn accel_raw_to_ms2(raw: i16) -> f32 {
+    (raw as f32 / SENS_2G) * G
+}
+
+/// Convert raw gyro (i16) to rad/s. For MPU6050 typical gyro scale at ±2000 deg/s -> 16.4 LSB/deg/s
+/// Adjust if you configured different full-scale range.
+fn gyro_raw_to_rads(raw: i16) -> f32 {
+    // lsb_per_dps e.g. 16.4 for ±2000 dps
+    (raw as f32 / 16.4) * (DEG2RAD)
+}
+
+/// Rotate body accel into ENU and remove gravity; returns (east, north, up) linear accel in m/s^2
+pub fn accel_body_to_enu_lin(body_acc: (f32,f32,f32), roll: f32, pitch: f32, yaw: f32) -> (f32,f32,f32) {
+    let (ax, ay, az) = body_acc;
+
+    let (sr, cr) = roll.sin_cos();
+    let (sp, cp) = pitch.sin_cos();
+    let (sy, cy) = yaw.sin_cos();
+
+    // Build R = Rz(yaw) * Ry(pitch) * Rx(roll)
+    // multiply R * a_body
+    let r11 =  cy*cp;
+    let r12 =  cy*sp*sr - sy*cr;
+    let r13 =  cy*sp*cr + sy*sr;
+
+    let r21 =  sy*cp;
+    let r22 =  sy*sp*sr + cy*cr;
+    let r23 =  sy*sp*cr - cy*sr;
+
+    let r31 = -sp;
+    let r32 =  cp*sr;
+    let r33 =  cp*cr;
+
+    // a_nav = R * a_body
+    let ax_nav = r11*ax + r12*ay + r13*az; // east
+    let ay_nav = r21*ax + r22*ay + r23*az; // north
+    let az_nav = r31*ax + r32*ay + r33*az; // up
+
+    // remove gravity (nav frame gravity points down in NED; in our ENU 'up' axis positive up)
+    // If accel measured gravity as +9.8 on sensor when static and z down convention used above,
+    // then az_nav will include +g in 'up' direction; subtract G.
+    let ax_lin = ax_nav;
+    let ay_lin = ay_nav;
+    let az_lin = az_nav - G; // linear acceleration in up axis
+
+    (ax_lin, ay_lin, az_lin)
+}
+
+#[embassy_executor::task]
+pub async fn mpu_task(events: Sender<'static, NoopRawMutex, Measurement, 4>, sink: DynamicSender<'static, Notification>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
+    let backoff = Backoff::from_millis(5);
+    let busref = RefCell::new(Some(bus));
+
+    backoff.forever().attempt::<_, (), ()>(async || {
+        let mut sensor = backoff.forever().attempt(async || {
+            info!("Initializing MPU");
+            match Mpu6050::new(busref.replace(None).unwrap(), Address::default()).await.map_err(|e| { e.i2c }) {
+                Err(i2c) => {
+                    busref.replace(Some(i2c));
+                    Err(())
+                },
+                Ok(mut sensor) => {
+                    match backoff.attempt(async || { mpu_init(&mut sensor).await }).await {
+                        Err(_) => {
+                            busref.replace(Some(sensor.release()));
+                            Err(())
+                        },
+                        Ok(_) => Ok(sensor)
+                    }
+                }
+            }
+        }).await?;
+
+        let sensor_ref = &mut sensor;
+
+        let alignment = {
+            info!("Locating earth...");
+            let mut accel_samples = [Vector3::default(); 100];
+            let mut gyro_samples = [Vector3::default(); 100];
+            for (idx, (accel_sample, gyro_sample)) in accel_samples.iter_mut().zip(gyro_samples.iter_mut()).enumerate() {
+                let (accel, gyro) = backoff.attempt(async || { sensor_ref.motion6().await }).await.unwrap();
+                *accel_sample = Vector3::new(accel_raw_to_ms2(accel.x()), accel_raw_to_ms2(accel.y()), accel_raw_to_ms2(accel.z()));
+                *gyro_sample = Vector3::new(gyro_raw_to_rads(gyro.x()), gyro_raw_to_rads(gyro.y()), gyro_raw_to_rads(gyro.z()));
+                info!("{idx}/200 {accel_sample:?} {gyro_sample:?}");
+                Timer::after_millis(5).await;
+            }
+
+            SensorAlignment::new(accel_samples.as_slice(), gyro_samples.as_slice())
+        };
+
+        info!("MPU is ready! earth={alignment:?}");
+        sink.send(Notification::CalibrationComplete).await;
+        let mut tilt = TiltEstimator::new(0.3);
+        //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();
+        loop {
+            match backoff.attempt(async || { sensor_ref.motion6().await }).await {
+                Ok((accel_data, gyro_data)) => {
+
+                    // Apply the initial alignment correction to put it into the body frame where X is forward
+                    let adjusted = alignment.apply(&Vector3::new(
+                        accel_raw_to_ms2(accel_data.x()),
+                        accel_raw_to_ms2(accel_data.y()),
+                        accel_raw_to_ms2(accel_data.z())
+                    ));
+                    let adjusted_gyro = alignment.apply_gyro(&Vector3::new(
+                        gyro_raw_to_rads(gyro_data.x()),
+                        gyro_raw_to_rads(gyro_data.y()),
+                        gyro_raw_to_rads(gyro_data.z())
+                    ));
+
+                    let filtered = Vector3::new(
+                        lowpass(prev_accel.x, adjusted.x, 0.3),
+                        lowpass(prev_accel.y, adjusted.y, 0.3),
+                        lowpass(prev_accel.z, adjusted.z, 0.3),
+                    );
+                    prev_accel = adjusted;
+
+                    // Apply current rotation and accel values to rotate our values back into the ENU frame
+                    tilt.update(0.01, (filtered.x, filtered.y, filtered.z), (adjusted_gyro.x, adjusted_gyro.y, adjusted_gyro.z));
+                    let (roll, pitch, yaw) = tilt.angles();
+                    let (ax_e, ay_n, _az_up) = accel_body_to_enu_lin((filtered.x,filtered.y,filtered.z), roll, pitch, yaw);
+
+                    let measured = Vector3::new(ax_e, ay_n, adjusted_gyro.z);
+                    events.send(
+                        Measurement::IMU(
+                            measured
+                        )
+                    ).await;
+                    Timer::after_millis(10).await;
+                },
+                Err(e) => {
+                    warn!("Failed to read MPU motion data! {e:?}");
+                    busref.replace(Some(sensor.release()));
+                    return Err(());
+                }
+            };
+        }
+    }).await.ok();
+}
+
+async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>) -> Result<(), Error<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>> {
+    let mut delay = Delay;
+    let backoff = Backoff::from_millis(10);
+    info!("Initializing MPU");
+    backoff.attempt(async || { sensor.initialize_dmp(&mut delay).await }).await?;
+    backoff.attempt(async || { sensor.set_digital_lowpass_filter(mpu6050_dmp::config::DigitalLowPassFilter::Filter3).await }).await?;
+    info!("Calibrating MPU");
+    backoff.attempt(async || {
+        // TODO: Store calibration data with set_accel_calibration, set_gyro_calibration
+        sensor.calibrate(&mut delay, &CalibrationParameters::new(
+            mpu6050_dmp::accel::AccelFullScale::G2,
+            mpu6050_dmp::gyro::GyroFullScale::Deg2000,
+            mpu6050_dmp::calibration::ReferenceGravity::Zero
+        )).await.map(|_| { Ok(()) })
+    }).await?
+}

src/tasks/render.rs

@@ -1,23 +1,26 @@
-use embassy_sync::{blocking_mutex::raw::NoopRawMutex, pubsub::Subscriber};
 use embassy_time::{Duration, Instant, Timer};
-use esp_hal::{rmt::ChannelCreator, Async, peripherals::GPIO5};
+use esp_hal::{gpio::AnyPin, rmt::ChannelCreator, Async};
 use esp_hal_smartled::{buffer_size_async, SmartLedsAdapterAsync};
-use figments::{hardware::Output, liber8tion::trig::sin8, prelude::Rectangle, render::Sample};
+use figments::{hardware::{Brightness, Output}, prelude::Hsv, surface::{BufferedSurfacePool, Surfaces}};
 use log::{info, warn};
-use rgb::Rgb;
+use rgb::Rgba;
 
-use crate::{display::BikeOutput, events::Notification};
+use crate::{display::{BikeOutput, BikeSpace}, events::DisplayControls};
 
+#[derive(Default)]
+pub struct Uniforms {
+    pub frame: usize,
+    pub primary_color: Hsv
+}
+
+//TODO: Import the bike surfaces from renderbug-prime, somehow make those surfaces into tasks
 
 #[embassy_executor::task]
-pub async fn render(mut events: Subscriber<'static, NoopRawMutex, Notification, 4, 4, 4>, rmt_channel: ChannelCreator<Async, 0>, gpio: GPIO5<'static>) {
+pub async fn render(rmt_channel: ChannelCreator<Async, 0>, gpio: AnyPin<'static>, surfaces: BufferedSurfacePool<Uniforms, BikeSpace, Rgba<u8>>, controls: &'static DisplayControls) {
     let rmt_buffer = [0u32; buffer_size_async(178)];
 
     let target = SmartLedsAdapterAsync::new(rmt_channel, gpio, rmt_buffer);
 
-    // Change this number to use a different number of LEDs
-    //let mut pixbuf = [Default::default(); 16];
-
     // Change this to adjust the power available; the USB spec says 500ma is the standard limit, but sometimes you can draw more from a power brick
     const POWER_MA : u32 = 500;
     
@@ -26,31 +29,25 @@ pub async fn render(mut events: Subscriber<'static, NoopRawMutex, Notification,
     const MAX_POWER_MW : u32 = POWER_VOLTS * POWER_MA;
 
     // This value is used as the 'seed' for rendering each frame, allowing us to do things like run the animation backwards, frames for double FPS, or even use system uptime for more human-paced animations
-    let mut frame = 0;
+    let mut uniforms = Uniforms::default();
 
-    //let mut target = BrightnessWriter::new(target, MAX_POWER_MW);
     let mut output = BikeOutput::new(target, MAX_POWER_MW);
 
     info!("Rendering started!");
 
     loop {
-        /*if let Some(evt) = events.try_next_message() {
-
-        }*/
         let start = Instant::now();
         //pixbuf.blank();
         output.blank().await.expect("Failed to blank framebuf");
 
-        // Render the frame to the pixbuf, while also calculating the power consumption as we go
-        for (coords, pix) in output.sample(&Rectangle::everything()) {
-            *pix = Rgb::new(sin8(coords.x.wrapping_mul(3).wrapping_add(coords.y.wrapping_mul(3)).wrapping_add(frame)), 0, 0);
-        }
+        surfaces.render_to(&mut output, &uniforms);
+
+        // Apply hardware controls
+        output.set_on(controls.on.load(core::sync::atomic::Ordering::Relaxed));
+        output.set_brightness(controls.brightness.load(core::sync::atomic::Ordering::Relaxed));
 
         // Finally, write out the rendered frame
-        //target.write(pixbuf.iter().cloned()).await.expect("Could not write frame");
-        //info!("frame");
         output.commit().await.expect("Failed to commit frame");
-        //info!("commit");
 
         let render_duration = Instant::now() - start;
         let render_budget = Duration::from_millis(16);
@@ -63,6 +60,6 @@ pub async fn render(mut events: Subscriber<'static, NoopRawMutex, Notification,
         }
 
         // Increment the frame counter
-        frame += 1;
+        uniforms.frame += 1;
     }
 }

src/tasks/ui.rs

@@ -0,0 +1,246 @@
+use embassy_sync::channel::DynamicReceiver;
+use embassy_time::{Duration, Instant, Timer};
+use figments::{liber8tion::interpolate::{ease_in_out_quad, Fract8}, prelude::*};
+use rgb::{Rgb, Rgba};
+use log::*;
+
+use crate::{display::BikeSpace, events::{DisplayControls, Notification, Scene}, shaders::*, tasks::render::Uniforms};
+
+
+pub struct Ui<S: Surface> {
+    // Background layer provides an always-running background for everything to draw on
+    background: S,
+
+    // Tail and panels provide content
+    tail: S,
+    panels: S,
+
+    // Notification layer sits on top of the content, and is used for transient event notifications (gps lost, wifi found, etc)
+    notification:S,
+
+    // Headlight and brakelight layers can only be overpainted by the bootsplash overlay layer
+    headlight: S,
+    brakelight: S,
+
+    // The overlay covers everything and is used to implement a power-on and power-off animation.
+    overlay:S,
+    headlight_on: bool,
+    brakelight_on: bool,
+    display: &'static DisplayControls
+}
+
+impl<S: Surface<Uniforms = Uniforms, CoordinateSpace = BikeSpace, Pixel = Rgba<u8>>> Ui<S> {
+    pub fn new(surfaces: &mut impl Surfaces<S::CoordinateSpace, Surface = S>, display: &'static DisplayControls) -> Self {
+        Self {
+            background: SurfaceBuilder::build(surfaces)
+                .rect(Rectangle::everything())
+                .shader(Background::default())
+                .opacity(32)
+                .visible(false)
+                .finish().unwrap(),
+            tail: SurfaceBuilder::build(surfaces)
+                .rect(Rectangle::new_from_coordinates(0, 5, 255, 5))
+                .opacity(96)
+                .shader(Tail::default())
+                .visible(false)
+                .finish().unwrap(),
+            panels: SurfaceBuilder::build(surfaces)
+                .rect(Rectangle::new_from_coordinates(0, 1, 255, 4))
+                .opacity(128)
+                .shader(Panel::default())
+                .visible(false)
+                .finish().unwrap(),
+            notification: SurfaceBuilder::build(surfaces)
+                .rect(Rectangle::everything())
+                .shader(Background::default())
+                .visible(false)
+                .finish().unwrap(),
+            headlight: SurfaceBuilder::build(surfaces)
+                .rect(Rectangle::new_from_coordinates(0, 0, 255, 0))
+                .shader(Headlight::default())
+                .visible(false)
+                .finish().unwrap(),
+            brakelight: SurfaceBuilder::build(surfaces)
+                .rect(Brakelight::rectangle())
+                .shader(Brakelight::default())
+                .visible(false)
+                .finish().unwrap(),
+            overlay: SurfaceBuilder::build(surfaces)
+                .rect(Rectangle::everything())
+                .visible(true)
+                .opacity(0)
+                .shader(Thinking::default())
+                .finish().unwrap(),
+            headlight_on: false,
+            brakelight_on: false,
+            display
+        }
+    }
+
+    pub async fn flash_notification_color(&mut self, color: Rgb<u8>) {
+        self.notification.set_shader(Background::from_color(color));
+        self.notification.set_opacity(255);
+        self.notification.set_visible(true);
+        Self::animate_duration(Duration::from_secs(3), |pct| {
+            self.notification.set_opacity(255 * pct);
+        }).await;
+        self.notification.set_visible(false);
+    }
+
+    async fn animate_duration<F: FnMut(Fract8)>(duration: Duration, mut f: F) {
+        let start = Instant::now();
+        let end = start + duration;
+        let full_duration = duration.as_millis() as f64;
+        loop {
+            let now = Instant::now();
+            if now > end {
+                break
+            } else {
+                let pct = (now - start).as_millis() as f64 / full_duration;
+                let frac_pct = (255.0 * pct) as u8;
+                f(frac_pct);
+                Timer::after_millis(5).await;
+            }
+        }
+    }
+
+    pub async fn startup_fade_sequence(&mut self) {
+        info!("Running startup fade sequence");
+        // Start with a completely transparent overlay, which then fades in over 1 second
+        self.overlay.set_opacity(0);
+        self.overlay.set_visible(true);
+        Self::animate_duration(Duration::from_secs(1), |pct| {
+            self.overlay.set_opacity(ease_in_out_quad(pct));
+        }).await;
+        // When the overlay is fully opaque and all lower layers will be hidden, turn them on
+        self.apply_scene(Scene::Startup).await;
+        // Then fade out the overlay over 3 seconds, allowing the base animations to be shown
+        Self::animate_duration(Duration::from_secs(3), |pct| {
+            self.overlay.set_opacity(ease_in_out_quad(255 - pct));
+        }).await;
+        self.overlay.set_visible(false);
+        info!("Fade sequence completed");
+    }
+
+    pub async fn set_headlight_on(&mut self, is_on: bool) {
+        if self.headlight_on != is_on {
+            self.brakelight_on = is_on;
+            if is_on {
+                info!("Turning on headlight");
+                self.headlight.set_opacity(0);
+                self.headlight.set_visible(true);
+                Self::animate_duration(Duration::from_secs(1), |pct| {
+                    self.headlight.set_opacity(pct);
+                }).await;
+                self.headlight.set_opacity(255);
+            } else {
+                info!("Turning off headlight");
+                Self::animate_duration(Duration::from_secs(1), |pct| {
+                    self.headlight.set_opacity(255 - pct);
+                }).await;
+                self.headlight.set_visible(false);
+            }
+        }
+    }
+
+    pub async fn set_brakelight_on(&mut self, is_on: bool) {
+        if self.brakelight_on != is_on {
+            self.brakelight_on = is_on;
+            if is_on {
+                info!("Turning on brakelight");
+                self.brakelight.set_opacity(0);
+                self.brakelight.set_visible(true);
+                Self::animate_duration(Duration::from_millis(300), |pct| {
+                    self.brakelight.set_opacity(pct);
+                }).await;
+                self.brakelight.set_opacity(255);
+            } else {
+                info!("Turning off brakelight");
+                Self::animate_duration(Duration::from_millis(300), |pct| {
+                    self.brakelight.set_opacity(255 - pct);
+                }).await;
+                self.brakelight.set_visible(false);
+            }
+        }
+    }
+
+    // TODO: Brakelight should only be toggled when actually braking or stationary
+    pub async fn apply_scene(&mut self, next_scene: Scene) {
+        info!("Activating scene {next_scene:?}");
+        match next_scene {
+            Scene::Startup => {
+                self.display.on.store(true, core::sync::atomic::Ordering::Relaxed);
+                self.display.brightness.store(255, core::sync::atomic::Ordering::Relaxed);
+                self.background.set_visible(true);
+                self.tail.set_visible(true);
+                self.panels.set_visible(true);
+            },
+            Scene::ParkedIdle => {
+                // Ensure the display is on
+                self.display.on.store(true, core::sync::atomic::Ordering::Relaxed);
+                // Hide the content; only notifications will remain
+                self.background.set_visible(true);
+                self.tail.set_visible(false);
+                self.panels.set_visible(false);
+                Self::animate_duration(Duration::from_secs(1), |pct| {
+                    self.display.brightness.store(128.scale8(255 - pct), core::sync::atomic::Ordering::Relaxed);
+                }).await;
+            },
+            Scene::ParkedLongTerm => {
+                // For long-term parking, we turn off the safety lights
+                self.set_headlight_on(false).await;
+                self.set_brakelight_on(false).await;
+                // Then we turn the display off completely
+                Self::animate_duration(Duration::from_secs(1), |pct| {
+                    self.display.brightness.store(255 - pct, core::sync::atomic::Ordering::Relaxed);
+                }).await;
+                self.display.on.store(false, core::sync::atomic::Ordering::Relaxed);
+            }
+            _ => unimplemented!()
+        }
+    }
+
+    pub async fn on_event(&mut self, event: Notification) {
+        match event {
+            // Apply the scene when it is changed
+            Notification::SceneChange(scene) => self.apply_scene(scene).await,
+
+            // TODO: We probably also want some events to indicate when the ESP has no calibration data or otherwise needs re-calibrated and is waiting for the bike to stand still
+            Notification::CalibrationComplete => self.flash_notification_color(Rgb::new(0, 255, 0)).await,
+            Notification::GPSLost => self.flash_notification_color(Rgb::new(255, 0, 0)).await,
+            Notification::GPSAcquired => self.flash_notification_color(Rgb::new(0, 255, 255)).await,
+            Notification::WifiConnected => self.flash_notification_color(Rgb::new(0, 0, 255)).await,
+            Notification::WifiDisconnected => self.flash_notification_color(Rgb::new(128, 0, 255)).await,
+
+            // Toggling head and brake lights
+            Notification::SetBrakelight(is_on) => self.set_brakelight_on(is_on).await,
+            Notification::SetHeadlight(is_on) => self.set_headlight_on(is_on).await
+
+            // Other event ideas:
+            // - Bike has crashed, or is laid down
+            // - Unstable physics right before crashing?
+            // - Turning left/right
+            // - BPM sync with phone app
+            // - GPS data is being synchronized with nextcloud/whatever
+            // - A periodic flash when re-initializing MPU and GPS, to indicate there might be a problem?
+            // - Bluetooth/BLE connect/disconnect events
+            // - Bike is waking up from stationary?
+        }
+    }
+}
+
+#[embassy_executor::task]
+pub async fn ui_main(events: DynamicReceiver<'static, Notification>, mut ui: Ui<BufferedSurface<Uniforms, BikeSpace, Rgba<u8>>>) {
+    // Run the fade sequence
+    ui.startup_fade_sequence().await;
+
+    // Briefly turn on the brakelight while the headlight turns on
+    ui.set_brakelight_on(true).await;
+    // Turn on the headlight when we are finished
+    ui.set_headlight_on(true).await;
+    // Turn off the brakelight
+    ui.set_brakelight_on(false).await;
+    loop {
+        ui.on_event(events.receive().await).await;
+    }
+}