animation: rewrite the apply() algorithm to use fewer timers and multiple targets at once

.cargo/config.toml

@@ -1,5 +1,5 @@
 [target.xtensa-esp32s3-none-elf]
-runner = "espflash flash --monitor --chip esp32s3 --partition-table ./partitions.csv"
+runner = "espflash flash --monitor --partition-table ./partitions.csv"
 
 [env]
 ESP_LOG="info"
@@ -11,7 +11,7 @@ ESP_RTOS_CONFIG_TICK_RATE_HZ="50"
 [build]
 rustflags = [
   "-C", "link-arg=-nostartfiles",
-  "-C", "force-frame-pointers"
+  "-Z", "stack-protector=all",
 ]
 
 target = "xtensa-esp32s3-none-elf"

Cargo.toml

@@ -15,9 +15,10 @@ real-output = []
 dual-core = []
 simulation = ["dep:rmp"]
 radio = [
-  "dep:bleps",
   "dep:esp-radio",
-  "dep:reqwless"
+  "dep:reqwless",
+  "dep:trouble-host",
+  "esp-rtos/esp-radio"
 ]
 motion = ["mpu", "gps"]
 max-usb-power = []
@@ -53,7 +54,6 @@ embassy-executor = { version = "0.9.0", features = [
 ] }
 embassy-time = { version = "0.5.0", features = ["log"] }
 esp-rtos = { version = "0.2.0", features = [
-  "esp-radio",
   "embassy",
   "esp-alloc",
   "esp32s3",
@@ -69,11 +69,11 @@ embassy-embedded-hal = "0.5.0"
 embedded-hal-async = "1.0.0"
 nalgebra = { version = "0.33.2", default-features = false, features = ["alloc", "libm"] }
 xtensa-lx-rt = { version = "*", features = ["float-save-restore"] }
-futures = { version = "0.3.31", default-features = false, features = ["async-await"] }
 micromath = "2.1.0"
 enumset = "1.1.10"
 enum-map = "2.7.3"
 portable-atomic = { version = "1.11", features = ["critical-section"] }
+embassy-futures = { version = "0.1.2", features = ["log"] }
 
 # Telemetry outputs
 esp-radio = { version = "*", optional = true, features = [
@@ -85,9 +85,9 @@ esp-radio = { version = "*", optional = true, features = [
   "coex",
   "unstable"
 ] }
-bleps =  { git = "https://github.com/bjoernQ/bleps", optional = true, package = "bleps", rev = "a5148d8ae679e021b78f53fd33afb8bb35d0b62e", features = [ "macros", "async"] }
+embassy-net = { version = "0.7.1", features = ["alloc", "dns", "medium-ethernet", "proto-ipv4", "tcp", "udp", "dhcpv4"] }
-embedded-graphics = { version = "0.8.1", features = ["nalgebra_support"] }
+reqwless = { version = "0.13.0", optional = true, features = ["log", "alloc"] }
-ssd1306 = { version = "0.10.0", features = ["async"], optional = true }
+trouble-host = { version = "0.5.1", optional = true, features = ["log"] }
 
 # Sensors
 nmea = { version = "0.7.0", optional = true, default-features = false, features = [
@@ -127,6 +127,9 @@ lto              = 'fat'
 opt-level        = 's'
 overflow-checks  = false
 
+[profile.dev.package.esp-radio]
+opt-level = 3
+
 [build-dependencies]
 image = "0.25"
 rmp = { path = "../msgpack-rust/rmp/" }

build.rs

@@ -198,7 +198,7 @@ fn write_sim_data() {
 
     generate_sim_data::<AnnotationReading>(&[&annotation_input], &annotation_output, 0);
     generate_sim_data::<GPSReading>(&[&gps_input], &gps_output, 0);
-    generate_sim_data::<IMUReading>(&[&accel_input, &gyro_input], &motion_output, 2);
+    generate_sim_data::<IMUReading>(&[&accel_input, &gyro_input], &motion_output, 3);
 
     let mut unified_fd = File::create(unified_output.clone()).unwrap();
 
@@ -237,7 +237,7 @@ fn write_sim_data() {
     }
 
     if unified_fd.metadata().unwrap().len() as usize >= data_size {
-        // FIXME: Need to implement data resampling
+        // FIXME: Need to implement automatic data resampling
         panic!("Simulation data is too big! Cannot fit {:#x} bytes into a partition with a size of {data_size:#x} bytes.", unified_fd.metadata().unwrap().len());
     }
 
@@ -252,6 +252,8 @@ fn parse_partition_number(n: &str) -> Option<usize> {
         Some(usize::from_str_radix(hex_offset, 16).unwrap())
     } else if let Some(mb_offset) = n.strip_suffix("M") {
         Some(mb_offset.parse::<usize>().unwrap() * 1024 * 1024)
+    } else if let Some(kb_offset) = n.strip_suffix("K") {
+        Some(kb_offset.parse::<usize>().unwrap() * 1024)
     } else {
         Some(n.parse().unwrap())
     }

partitions.csv

@@ -4,4 +4,4 @@
 nvs,      data, nvs,     ,        0x6000,
 phy_init, data, phy,     ,        0x1000,
 factory,  app,  factory, ,        2M,
-sim,      data, undefined,,       6M,
+sim,      data, undefined,,       5000K,

src/animation.rs

@@ -1,29 +1,30 @@
-use embassy_time::{Duration, Timer};
+use embassy_time::{Duration, Instant, Timer};
-use figments::surface::Surface;
+use esp_rtos::CurrentThreadHandle;
+use figments::{liber8tion::interpolate::Fract8, surface::Surface};
 use figments_render::output::Brightness;
-use core::{fmt::Debug, ops::{Deref, DerefMut}};
+use core::{fmt::Debug, mem::MaybeUninit, ops::{Deref, DerefMut}};
 use log::*;
 
 use crate::graphics::display::DisplayControls;
 
 #[derive(Default, Debug, Clone, Copy)]
-pub struct Animation {
+pub struct Animation<T> {
-    from: Option<u8>,
+    from: Option<T>,
-    to: Option<u8>,
+    to: Option<T>,
     duration: Duration
 }
 
-pub trait AnimationActor {
+pub trait AnimationActor<T> {
-    fn get_opacity(&self) -> u8;
+    fn get_value(&self) -> T;
-    fn set_opacity(&mut self, opacity: u8);
+    fn set_value(&mut self, value: T);
 }
 
-impl<S: Surface> AnimationActor for S {
+impl<S: Surface> AnimationActor<Fract8> for S {
-    fn get_opacity(&self) -> u8 {
+    fn get_value(&self) -> Fract8 {
-        0
+        unimplemented!()
     }
 
-    fn set_opacity(&mut self, opacity: u8) {
+    fn set_value(&mut self, opacity: Fract8) {
         self.set_opacity(opacity);
     }
 }
@@ -31,28 +32,48 @@ impl<S: Surface> AnimationActor for S {
 #[derive(Debug)]
 pub struct AnimDisplay<'a>(pub &'a mut DisplayControls);
 
-impl<'a> AnimationActor for AnimDisplay<'a> {
+impl<'a> AnimationActor<Fract8> for AnimDisplay<'a> {
-    fn get_opacity(&self) -> u8 {
+    fn get_value(&self) -> Fract8 {
         self.0.brightness()
     }
 
-    fn set_opacity(&mut self, opacity: u8) {
+    fn set_value(&mut self, opacity: Fract8) {
         self.0.set_brightness(opacity);
     }
 }
 
-impl<S: Surface> AnimationActor for AnimatedSurface<S> {
+impl<S: Surface> AnimationActor<Fract8> for AnimatedSurface<S> {
-    fn get_opacity(&self) -> u8 {
+    fn get_value(&self) -> Fract8 {
         self.opacity
     }
 
-    fn set_opacity(&mut self, opacity: u8) {
+    fn set_value(&mut self, opacity: Fract8) {
         self.surface.set_opacity(opacity);
         self.opacity = opacity;
     }
 }
 
-impl Animation {
+struct Slot<'a, T> {
+    from: T,
+    to: T,
+    cur_step: T,
+    step_time: Duration,
+    next_update: Instant,
+    target: &'a mut dyn AnimationActor<T>
+}
+
+impl<'a, T: Debug> core::fmt::Debug for Slot<'a, T> {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        f.debug_struct("Slot")
+            .field("from", &self.from)
+            .field("to", &self.to)
+            .field("cur_step", &self.cur_step)
+            .field("step_time", &self.step_time)
+            .field("next_update", &self.next_update).finish()
+    }
+}
+
+impl Animation<Fract8> {
     pub const fn new() -> Self {
         Self {
             from: None,
@@ -60,61 +81,111 @@ impl Animation {
             duration: Duration::from_ticks(0)
         }
     }
-    pub const fn from(self, from: u8) -> Self {
+
+    pub const fn from(self, from: Fract8) -> Self {
         Self {
             from: Some(from),
-            to: self.to,
+            ..self
-            duration: self.duration
         }
     }
 
-    pub const fn to(self, to: u8) -> Self {
+    pub const fn to(self, to: Fract8) -> Self {
         Self {
-            from: self.from,
             to: Some(to),
-            duration: self.duration
+            ..self
         }
     }
 
     pub const fn duration(self, duration: Duration) -> Self {
         Self {
-            from: self.from,
+            duration,
-            to: self.to,
+            ..self
-            duration
         }
     }
+}
 
-    pub async fn apply<S: AnimationActor>(&self, sfc: &mut S) {
+impl Animation<Fract8> {
-        let from = if let Some(val) = self.from {
+    pub async fn apply<const ACTOR_COUNT: usize>(&self, actors: [&mut dyn AnimationActor<Fract8>; ACTOR_COUNT]) {
-            val
-        } else {
-            sfc.get_opacity()
-        };
-        let to = if let Some(val) = self.to {
-            val
-        } else {
-            sfc.get_opacity()
-        };
-        let steps = from.abs_diff(to);
-        if steps == 0 {
-            return;
-        }
-        let step_time = self.duration / steps.into();
-        trace!("fade={self:?} steps={steps} time={step_time}");
-        if from > to {
-            let range = (to..=from).rev();
-            for opacity in range {
-                sfc.set_opacity(opacity);
-                Timer::after(step_time).await;
-            }
-        } else {
-            let range = from..=to;
 
-            for opacity in range {
+        let mut now = Instant::now();
-                sfc.set_opacity(opacity);
+        trace!("start now={now:?} ACTOR_COUNT={ACTOR_COUNT}");
-                Timer::after(step_time).await;
+
+        let mut actors = actors.into_iter();
+        let mut animators: [Slot<Fract8>; ACTOR_COUNT] = core::array::from_fn(|_| {
+            let target = actors.next().unwrap();
+            let from = if let Some(val) = self.from {
+                val
+            } else {
+                target.get_value()
+            };
+            let to = if let Some(val) = self.to {
+                val
+            } else {
+                target.get_value()
+            };
+            let steps = to.abs_diff(from);
+
+            let step_time = if steps == Fract8::MIN {
+                Duration::from_ticks(0)
+            } else {
+                (self.duration / steps.to_raw().into()).max(Duration::from_millis(1))
+            };
+            Slot {
+                from,
+                to,
+                cur_step: from,
+                step_time,
+                next_update: now,
+                target
             }
+        });
+
+        trace!("animators={animators:?}");
+
+        loop {
+            // Find the next shortest delay
+            let mut next_keyframe_time = animators[0].next_update;
+            let mut finished = false;
+            for animator in &mut animators {
+                if animator.step_time.as_ticks() == 0 {
+                    continue;
+                }
+                if animator.next_update <= now {
+                    animator.next_update += animator.step_time;
+                    animator.cur_step = if animator.to > animator.from {
+                        animator.cur_step + Fract8::from_raw(1)
+                    } else {
+                        animator.cur_step - Fract8::from_raw(1)
+                    };
+
+                    if (animator.to > animator.from && animator.cur_step >= animator.to) ||
+                        (animator.to <= animator.from && animator.cur_step <= animator.to) {
+                        finished = true;
+                    }
+
+                    /*if animator.cur_step == animator.from || animator.cur_step == animator.to {
+                        finished = true;
+                    }*/
+
+                    animator.target.set_value(animator.cur_step);
+                }
+
+                if next_keyframe_time <= now || animator.next_update < next_keyframe_time {
+                    next_keyframe_time = animator.next_update;
+                }
+            }
+
+            if finished {
+                break;
+            }
+
+            let keyframe_delay = next_keyframe_time - now;
+            trace!("delay {keyframe_delay:?}");
+            Timer::after(keyframe_delay).await;
+            now += keyframe_delay;
         }
+
+        trace!("finished animators={animators:?}");
     }
 }
 
@@ -122,7 +193,7 @@ impl Animation {
 pub struct AnimatedSurface<S: Surface> {
     surface: S,
     is_on: bool,
-    opacity: u8
+    opacity: Fract8
 }
 
 impl<S: Surface> Deref for AnimatedSurface<S> {
@@ -145,7 +216,7 @@ impl<S: Surface + Debug> From<S> for AnimatedSurface<S> {
         AnimatedSurface { 
             surface,
             is_on: false,
-            opacity: 255
+            opacity: Fract8::MAX
         }
     }
 }
@@ -155,11 +226,11 @@ impl<S: Surface + Debug> AnimatedSurface<S> {
         if self.is_on != is_on {
             let anim = if is_on {
                 self.surface.set_visible(true);
-                Animation::default().duration(Duration::from_secs(1)).from(0).to(255)
+                Animation::default().duration(Duration::from_secs(1)).from(Fract8::MIN).to(Fract8::MAX)
             } else {
-                Animation::default().duration(Duration::from_secs(1)).from(255).to(0)
+                Animation::default().duration(Duration::from_secs(1)).from(Fract8::MAX).to(Fract8::MIN)
             };
-            anim.apply(&mut self.surface).await;
+            anim.apply([&mut self.surface]).await;
             self.is_on = true;
             if !is_on {
                 self.surface.set_visible(false);

src/bin/main.rs

@@ -7,13 +7,13 @@
 )]
 
 
-use core::ptr::addr_of_mut;
+use core::{num::{self, Wrapping}, ptr::addr_of_mut};
 
-use alloc::sync::Arc;
+use alloc::{string::String, sync::Arc};
 use embassy_executor::Spawner;
 use embassy_time::{Instant, Timer};
 
-use esp_hal::{gpio::{Output, OutputConfig, Pin}, time::Rate};
+use esp_hal::{gpio::{Output, OutputConfig, Pin}, time::Rate, xtensa_lx::debug_break};
 use esp_hal::{
     clock::CpuClock, system::{AppCoreGuard, CpuControl, Stack}, timer::{systimer::SystemTimer, timg::{TimerGroup, Wdt}}
 };
@@ -22,7 +22,7 @@ use embassy_sync::{
     pubsub::PubSubChannel,
     blocking_mutex::raw::NoopRawMutex
 };
-
+use static_cell::ConstStaticCell;
 use log::*;
 use renderbug_embassy::{events::Prediction, graphics::display::DisplayControls, logging::RenderbugLogger, tasks::{oled::{OledUI, OledUiSurfacePool, oled_ui}, safetyui::{SafetyUi, safety_ui_main}, ui::UiSurfacePool}};
 use renderbug_embassy::events::Measurement;
@@ -45,9 +45,6 @@ extern crate alloc;
 // For more information see: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
 esp_bootloader_esp_idf::esp_app_desc!();
 
-static STATIC_HI_EXEC: StaticCell<InterruptExecutor<2>> = StaticCell::new();
-static CORE2_EXEC: StaticCell<Executor> = StaticCell::new();
-static mut CORE2_STACK: Stack<16384> = Stack::new();
 #[cfg(feature="radio")]
 static WIFI_INIT: StaticCell<esp_radio::Controller<'static>> = StaticCell::new();
 
@@ -83,14 +80,14 @@ async fn main(spawner: Spawner) {
     let timer0 = TimerGroup::new(peripherals.TIMG0);
     let mut wdt = timer0.wdt;
     wdt.set_timeout(esp_hal::timer::timg::MwdtStage::Stage0, esp_hal::time::Duration::from_secs(5));
-    //wdt.enable();
+    wdt.enable();
-
-    let swi = SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
-    //let hi_exec = STATIC_HI_EXEC.init(InterruptExecutor::new(swi.software_interrupt2));
-    //let hi_spawn = hi_exec.start(esp_hal::interrupt::Priority::max());
 
+    // Spawn the rendering task as soon as possible so it can start pushing pixels
     spawner.must_spawn(renderbug_embassy::tasks::render::render(peripherals.RMT, peripherals.GPIO5.degrade(), surfaces, safety_surfaces, display_controls, wdt));
 
+    // Wait one scheduler tick for the rendering task to get initialized
+    Timer::after_ticks(1).await;
+
     #[cfg(feature="motion")]
     {
         use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
@@ -131,7 +128,7 @@ async fn main(spawner: Spawner) {
         let mut storage = SharedFlash::new(FlashStorage::new());
         let mut buf = [8; 1024];
         let partitions = esp_bootloader_esp_idf::partitions::read_partition_table(&mut storage, &mut buf).unwrap();
-        for sim_data in SimDataTable::open(storage, partitions).expect("Could not find partition for sim data!") {
+        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_embassy::tasks::simulation::simulation_task(sim_data, motion_bus.dyn_sender())).is_err() {
@@ -141,15 +138,23 @@ async fn main(spawner: Spawner) {
     }
 
     #[cfg(feature="radio")]
-    let wifi_init = {
+    let (wifi, network_device, ble) = {
         info!("Configuring wifi");
-        WIFI_INIT.init_with(|| {esp_radio::init().expect("Failed to initialize radio controller")})
+        esp_radio::wifi_set_log_verbose();
+        let wifi_init = WIFI_INIT.init_with(|| {esp_radio::init().expect("Failed to initialize radio controller")});
+
+        let ble = esp_radio::ble::controller::BleConnector::new(wifi_init, peripherals.BT, esp_radio::ble::Config::default()).unwrap();
+
+        let (wifi, interfaces) = esp_radio::wifi::new(wifi_init, peripherals.WIFI, esp_radio::wifi::Config::default())
+            .expect("Failed to initialize WIFI!");
+
+        (wifi, interfaces.sta, ble)
     };
 
-    info!("Starting core 2");
-
     let core2_main = |spawner: Spawner| {
-        static PREDICTIONS: StaticCell<PubSubChannel<NoopRawMutex, Prediction, 15, 5, 1>> = StaticCell::new();
+        info!("Starting application tasks");
+
+        static PREDICTIONS: StaticCell<PubSubChannel<NoopRawMutex, Prediction, 15, 6, 1>> = StaticCell::new();
         let predictions = PREDICTIONS.init(PubSubChannel::new());
 
         #[cfg(not(feature="demo"))]
@@ -173,8 +178,26 @@ async fn main(spawner: Spawner) {
 
         #[cfg(feature="radio")]
         {
-            info!("Launching networking stack");
+            use embassy_net::StackResources;
-            spawner.must_spawn(renderbug_embassy::tasks::wifi::wireless_task(predictions.dyn_subscriber().unwrap(), wifi_init, peripherals.WIFI));
+            use esp_hal::rng::Rng;
+            use static_cell::ConstStaticCell;
+
+            info!("Setting up networking stack");
+            static RESOURCES: ConstStaticCell<StackResources<5>> = ConstStaticCell::new(StackResources::new());
+            let config = embassy_net::Config::dhcpv4(Default::default());
+            let seed = Rng::new().random() as i32;
+            let (stack, runner) = embassy_net::new(network_device, config, RESOURCES.take(), seed as u64);
+            info!("Launching network services");
+            //spawner.must_spawn(renderbug_embassy::tasks::wifi::net_task(runner));
+
+            info!("Starting connectivity task");
+            //spawner.must_spawn(renderbug_embassy::tasks::wifi::wifi_connect_task(wifi, stack, motion_bus.dyn_sender()));
+
+            info!("Launching HTTP telemetry");
+            //spawner.must_spawn(renderbug_embassy::tasks::wifi::http_telemetry_task(predictions.dyn_subscriber().unwrap(), stack));
+
+            info!("Starting BLE services");
+            spawner.must_spawn(renderbug_embassy::tasks::ble::ble_task(ble, predictions.dyn_subscriber().unwrap(), spawner));
         }
 
         #[cfg(feature="dual-core")]
@@ -194,13 +217,21 @@ async fn main(spawner: Spawner) {
     };
 
     #[cfg(feature="dual-core")]
-    esp_rtos::start_second_core(peripherals.CPU_CTRL, swi.software_interrupt0, swi.software_interrupt1, unsafe { &mut *addr_of_mut!(CORE2_STACK) }, || {
+    {
-        let exec = CORE2_EXEC.init_with(|| { Executor::new() });
+        static mut CORE2_STACK: Stack<16384> = Stack::new();
-        exec.run(core2_main);
+        let swi = SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
-    });
+        esp_rtos::start_second_core(peripherals.CPU_CTRL, swi.software_interrupt0, swi.software_interrupt1, unsafe { &mut *addr_of_mut!(CORE2_STACK) }, || {
+            info!("Second CPU core started");
+            static CORE2_EXEC: StaticCell<Executor> = StaticCell::new();
+            let exec = CORE2_EXEC.init_with(|| { Executor::new() });
+            exec.run(core2_main);
+        });
+    }
 
     #[cfg(not(feature="dual-core"))]
     core2_main(spawner);
+
+    info!("Ready to rock and roll");
 }
 
 #[embassy_executor::task]
@@ -216,7 +247,11 @@ async fn wdt_task(mut wdt: Wdt<esp_hal::peripherals::TIMG1<'static>>) {
 
 #[embassy_executor::task]
 async fn print_telemetry(mut events: DynSubscriber<'static, Prediction>) {
+    info!("telemetry ready");
+    let mut num_events = Wrapping(0usize);
     loop {
-        info!("predict={:?}", events.next_message_pure().await);
+        let next = events.next_message_pure().await;
+        trace!("idx={} predict={next:?}", num_events.0);
+        num_events += 1;        
     }
 }

src/ego/engine.rs

@@ -1,4 +1,4 @@
-use embassy_sync::{channel::DynamicSender, pubsub::DynPublisher};
+use embassy_sync::pubsub::DynPublisher;
 use embassy_time::{Duration, Instant};
 use nalgebra::{Rotation3, Vector2, Vector3, ComplexField, RealField};
 use log::*;

src/events.rs

@@ -74,10 +74,14 @@ pub enum SensorSource {
     IMU,
     GPS,
 
+    // Connectivity related
+    Wifi,
+
     // Fusion outputs
     GravityReference,
     ForwardsReference,
     Location,
+    Cloud,
 
     // Simulated sensors
     Demo,

src/graphics/oled_ui.rs

@@ -115,6 +115,10 @@ const SENSOR_IMAGES: &[SensorImage] = &[
         source: SensorSource::Location,
         on: &images::LOCATION_ON, off: &images::LOCATION_OFF,
     },
+    SensorImage {
+        source: SensorSource::Wifi,
+        on: &images::ONLINE_CONNECTING, off: &images::OFFLINE
+    },
     #[cfg(feature="demo")]
     SensorImage {
         source: SensorSource::Demo,

src/graphics/ssd1306.rs

@@ -38,7 +38,7 @@ impl SsdPixel {
     }
 }
 
-const DITHER_MAP: [u32;15] = [
+const DITHER_MAP: [u16;15] = [
     0b1000_0000_0000_0000,
     0b1000_0000_0010_0000,
     0b1010_0000_0010_0000,
@@ -62,9 +62,9 @@ impl AdditivePixelSink<BinaryColor> for SsdPixel {
             0 => (),
             255 => self.set_pixel(pixel),
             _ => {
-                let dither_value = DITHER_MAP[opacity as usize / 16];
+                let dither_value = DITHER_MAP[opacity as usize / 17];
                 let dither_x = self.coords.x % 4;
-                let dither_y = self.coords.x % 4;
+                let dither_y = self.coords.y % 4;
                 if dither_value.shr(dither_x).shr(dither_y * 4).bitand(0x01) == 1 {
                     self.set_pixel(pixel);
                 }

src/lib.rs

@@ -9,6 +9,9 @@ pub mod idle;
 pub mod logging;
 pub mod graphics;
 
+#[cfg(feature="simulation")]
+pub mod storage;
+
 #[cfg(feature="simulation")]
 pub mod simdata;
 

src/simdata.rs

@@ -97,7 +97,7 @@ impl RmpData for StreamHeader {
         }
     }
 
-    fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
+    fn write_rmp<Writer: RmpWrite>(&self, _writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
         todo!()
     }
 }

src/storage.rs

@@ -0,0 +1,122 @@
+use core::{cell::RefCell, fmt::Formatter};
+
+use alloc::rc::Rc;
+use embedded_storage::{ReadStorage, Storage};
+use log::*;
+use rmp::decode::{RmpRead, RmpReadErr};
+
+#[derive(Debug)]
+pub struct SharedFlash<S> {
+    storage: Rc<RefCell<S>>
+}
+
+impl<S> Clone for SharedFlash<S> {
+    fn clone(&self) -> Self {
+        Self {
+            storage: Rc::clone(&self.storage)
+        }
+    }
+}
+
+impl<S> SharedFlash<S> {
+    pub fn new(storage: S) -> Self {
+        Self {
+            storage: Rc::new(RefCell::new(storage))
+        }
+    }
+}
+
+impl<S: Storage> Storage for SharedFlash<S> {
+    fn write(&mut self, offset: u32, bytes: &[u8]) -> Result<(), Self::Error> {
+        self.storage.borrow_mut().write(offset, bytes)
+    }
+}
+
+impl<S: ReadStorage> ReadStorage for SharedFlash<S> {
+    type Error = S::Error;
+
+    fn read(&mut self, offset: u32, bytes: &mut [u8]) -> Result<(), Self::Error> {
+        self.storage.borrow_mut().read(offset, bytes)
+    }
+
+    fn capacity(&self) -> usize {
+        self.storage.borrow().capacity()
+    }
+}
+
+#[derive(Debug)]
+pub enum RangeReadError<E> {
+    OutOfData,
+    Storage(E)
+}
+impl<E: core::fmt::Debug + 'static> RmpReadErr for RangeReadError<E> {}
+
+impl<E> core::fmt::Display for RangeReadError<E> {
+    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
+        f.write_str("RmpErr")
+    }
+}
+
+#[derive(Debug)]
+pub struct RangeReader<S> {
+    storage: S,
+    start: usize,
+    end: usize,
+    offset: usize
+}
+
+impl<S: ReadStorage> RangeReader<S> {
+    pub const fn new(storage: S, start: usize, end: usize) -> Self {
+        assert!(start <= end);
+        // TODO: Should add bounds checking since we will know the size of the chunk already
+        Self {
+            storage,
+            start,
+            end,
+            offset: 0
+        }
+    }
+
+    pub fn seek(&mut self, offset: usize) -> Result<(), RangeReadError<S::Error>> {
+        self.offset += offset;
+        if self.offset > self.end {
+            Err(RangeReadError::OutOfData)
+        } else {
+            Ok(())
+        }
+    }
+
+    pub fn subset(&self, size: usize) -> Result<Self, RangeReadError<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 {
+            Err(RangeReadError::OutOfData)
+        } else {
+            Ok(Self {
+                storage: self.storage.clone(),
+                start: self.offset + self.start,
+                end: self.start + self.offset + size,
+                offset: 0
+            })
+        }
+    }
+}
+
+impl<S: ReadStorage> RmpRead for RangeReader<S> where S::Error: core::fmt::Debug + 'static {
+    type Error = RangeReadError<S::Error>;
+
+    fn read_exact_buf(&mut self, buf: &mut [u8]) -> Result<(), RangeReadError<S::Error>> {
+        let pos = self.start + self.offset;
+        if pos > self.end {
+            Err(RangeReadError::OutOfData)
+        } else {
+            assert!(pos + buf.len() <= self.end);
+            match self.storage.read(pos as u32, buf) {
+                Ok(_) => {
+                    self.offset += buf.len();
+                    Ok(())
+                },
+                Err(err) => Err(RangeReadError::Storage(err))
+            }
+        }
+    }
+}

src/tasks/ble.rs

@@ -0,0 +1,224 @@
+
+use core::ptr::slice_from_raw_parts;
+
+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::{ConstStaticCell, StaticCell};
+use trouble_host::{attribute, prelude::*, types::gatt_traits::FromGattError};
+use log::*;
+
+use crate::{backoff::Backoff, events::Prediction};
+
+#[gatt_server]
+struct SerialServer {
+    serial_service: SerialService,
+    location_service: LocationService,
+    renderbug_service: RenderbugService
+}
+
+#[gatt_service(uuid="00000000-f43a-48c4-8411-efe6a38a5d75")]
+struct RenderbugService {
+    // 32 bit float, 0 exponent, m/s velocity unit
+    #[descriptor(uuid=descriptors::CHARACTERISTIC_PRESENTATION_FORMAT, read, value = [0x14, 0x0, 0x27, 0x12, 0x01, 0x00, 0x00])]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "Speed (m/s)")]
+    #[characteristic(uuid="00000001-f43a-48c4-8411-efe6a38a5d75", notify)]
+    speed: f32,
+
+    // 32 bit float, 0 exponent, unitless
+    #[descriptor(uuid=descriptors::CHARACTERISTIC_PRESENTATION_FORMAT, read, value = [0x14, 0x0, 0x27, 0x00, 0x01, 0x00, 0x00])]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "Latitude")]
+    #[characteristic(uuid="00000002-f43a-48c4-8411-efe6a38a5d75", notify)]
+    latitude: f32,
+
+    // 32 bit float, 0 exponent, unitless
+    #[descriptor(uuid=descriptors::CHARACTERISTIC_PRESENTATION_FORMAT, read, value = [0x14, 0x0, 0x27, 0x00, 0x01, 0x00, 0x00])]
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "Longitude")]
+    #[characteristic(uuid="00000003-f43a-48c4-8411-efe6a38a5d75", notify)]
+    longitude: f32,
+}
+
+#[gatt_service(uuid=service::LOCATION_AND_NAVIGATION)]
+struct LocationService {
+    #[characteristic(uuid=characteristic::LOCATION_AND_SPEED, notify)]
+    location_data: LocationData,
+    #[characteristic(uuid=characteristic::LN_FEATURE, read, value=0b0000101)] // Location and speed fields
+    features: u8,
+}
+
+#[derive(Default, Debug)]
+struct LocationData {
+    coordinates: [i32; 2],
+    velocity: u16
+}
+
+impl FixedGattValue for LocationData {
+    fn as_gatt(&self) -> &[u8] {
+        let databuf = [0;Self::SIZE];
+        unsafe { core::slice::from_raw_parts(self as *const Self as *const u8, Self::SIZE) }
+    }
+    
+    const SIZE: usize = core::mem::size_of::<Self>();
+    
+    fn from_gatt(data: &[u8]) -> Result<Self, FromGattError> {
+        if data.len() != Self::SIZE {
+            Err(FromGattError::InvalidLength)
+        } else {
+            // SAFETY
+            // - Pointer is considered "valid" as per the rules outlined for validity in std::ptr v1.82.0
+            // - Pointer was generated from a slice of bytes matching the size of the type implementing Primitive, and all types implementing Primitive are valid for all possible configurations of bits
+            // - Primitive trait is constrained to require Copy
+            unsafe { Ok((data.as_ptr() as *const Self).read_unaligned()) }
+        }
+    }
+}
+
+#[gatt_service(uuid="6E400001-B5A3-F393-E0A9-E50E24DCCA9E")]
+struct SerialService {
+    // The outside world writes here
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "RX")]
+    #[characteristic(uuid = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E", write)]
+    rx: heapless::String<128>,
+
+    // The outside world reads from here
+    #[descriptor(uuid = descriptors::MEASUREMENT_DESCRIPTION, read, value = "TX")]
+    #[characteristic(uuid = "6E400003-B5A3-F393-E0A9-E50E24DCCA9E", notify)]
+    tx: heapless::String<128>
+}
+
+#[embassy_executor::task]
+async fn client_prediction_task(mut src: DynSubscriber<'static, Prediction>, sink: Publisher<'static, NoopRawMutex, Prediction, 5, 1, 1>) {
+    debug!("Started BLE client prediction stream");
+    loop {
+        sink.publish(src.next_message_pure().await).await;
+    }
+}
+
+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();
+
+#[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 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 Host { mut peripheral, mut runner, .. } = stack.build();
+
+    let client_predictions = STATIC_CLIENT_PREDICTIONS.init(PubSubChannel::new());
+    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
+    let _ = embassy_futures::join::join(
+        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!("Waiting for connection");
+                match advertiser.accept().await.and_then(|raw| { raw.with_attribute_server(server) }) {
+                    Ok(conn) => {
+                        if spawner.spawn(ble_connection(conn, server, client_predictions.dyn_subscriber().unwrap())).is_err() {
+                            error!("Unable to spawn task for handling BLE connection! Please increase the embassy task pool size!");
+                        }
+                    },
+                    Err(e) => {
+                        error!("Failed to accept new connection: {e:?}");
+                    },
+                }
+            }
+        }
+    ).await;
+}
+
+#[embassy_executor::task]
+async fn ble_connection(conn: GattConnection<'static, 'static, DefaultPacketPool>, server: &'static SerialServer<'static>, mut predictions: DynSubscriber<'static, Prediction>) {
+    info!("Started handling new connection with {:?}", conn.raw().peer_identity());
+    let mut location_data = LocationData::default();
+    loop {
+        let next = embassy_futures::select::select(conn.next(), predictions.next_message_pure()).await;
+        match next {
+            Either::First(gatt_event) => {
+                match gatt_event {
+                    GattConnectionEvent::Disconnected { reason } => {
+                        warn!("BLE Disconnected: {reason:?}");
+                        return;
+                    },
+                    GattConnectionEvent::Gatt { event } => {
+                        if let GattEvent::Write(event) = event {
+                            if event.handle() == server.serial_service.rx.handle {
+                                let buf = event.value(&server.serial_service.rx).unwrap();
+                                info!("Received serial data {buf:?}");
+                            }
+                        }
+                    },
+                    _ => trace!("GATT event")
+                }
+            },
+            Either::Second(prediction) => {
+                let evt_str = heapless::format!("{prediction:?}\n").unwrap();
+                server.serial_service.tx.notify(&conn, &evt_str).await.unwrap();
+                let update_location = match prediction {
+                    Prediction::Velocity(v) => {
+                        location_data.velocity = (v * 100.0) as u16;
+                        server.renderbug_service.speed.notify(&conn, &v).await.unwrap();
+                        true
+                    },
+                    Prediction::Location(coords) => {
+                        location_data.coordinates[0] = (coords.x * 1e7) as i32;
+                        location_data.coordinates[1] = (coords.y * 1e7) as i32;
+                        server.renderbug_service.latitude.notify(&conn, &(coords.x as f32)).await.unwrap();
+                        server.renderbug_service.longitude.notify(&conn, &(coords.y as f32)).await.unwrap();
+                        true
+                    },
+                    _ => false
+                };
+
+                if update_location {
+                    server.location_service.location_data.notify(&conn, &location_data).await.unwrap();
+                    // TODO: write out NMEA sentences
+                    //let mut nmea_str = heapless::String::from_utf8("foo");
+                    //server.nmea_service.tx.notify(&conn, "foo").await.unwrap();
+                }
+            }
+        }
+    }
+}

src/tasks/mod.rs

@@ -4,6 +4,8 @@ pub mod mpu;
 pub mod gps;
 #[cfg(feature="radio")]
 pub mod wifi;
+#[cfg(feature="radio")]
+pub mod ble;
 #[cfg(feature="simulation")]
 pub mod simulation;
 #[cfg(feature="demo")]

src/tasks/oled_render.rs

@@ -9,7 +9,7 @@ use crate::{backoff::Backoff, graphics::ssd1306::SsdOutput, tasks::oled::{Locked
 
 
 #[embassy_executor::task]
-pub async fn oled_render(mut output: SsdOutput, surfaces: OledUiSurfacePool, uniforms: LockedUniforms) {
+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;

src/tasks/render.rs

@@ -10,7 +10,7 @@ use crate::graphics::display::NUM_PIXELS;
 use crate::{graphics::display::{BikeOutput, DisplayControls, Uniforms}, tasks::ui::UiSurfacePool};
 
 #[embassy_executor::task]
-pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'static>, surfaces: UiSurfacePool, safety_surfaces: UiSurfacePool, mut controls: DisplayControls, mut wdt: Wdt<esp_hal::peripherals::TIMG0<'static>>) {
+pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'static>, mut surfaces: UiSurfacePool, mut safety_surfaces: UiSurfacePool, mut controls: DisplayControls, mut wdt: Wdt<esp_hal::peripherals::TIMG0<'static>>) {
     let frequency: Rate = Rate::from_mhz(80);
     let rmt = Rmt::new(rmt, frequency)
         .expect("Failed to initialize RMT").into_async();

src/tasks/safetyui.rs

@@ -4,7 +4,6 @@ use figments::prelude::*;
 use figments_render::output::Brightness;
 use rgb::Rgba;
 use core::fmt::Debug;
-use futures::join;
 use log::*;
 
 use crate::{animation::{AnimDisplay, AnimatedSurface, Animation}, events::{Personality, Prediction}, graphics::{display::{DisplayControls, SegmentSpace, Uniforms}, shaders::*}, tasks::ui::UiSurfacePool};
@@ -84,10 +83,10 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
         self.headlight.set_visible(true);
         self.brakelight.set_opacity(0);
         self.brakelight.set_visible(true);
-        join!(
+        embassy_futures::join::join(
             fade_in.apply(&mut self.headlight),
             fade_in.apply(&mut self.brakelight)
-        );
+        ).await;
 
         info!("Fade out overlay");
         TURN_OFF.apply(&mut self.overlay).await;
@@ -100,17 +99,17 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
             Personality::Active => {
                 // FIXME: These should be a Off/Low/High enum, so the stopping brake looks different from the dayrunning brake.
                 warn!("Active personality: Turning on safety lights");
-                join!(
+                embassy_futures::join::join(
                     TURN_ON.apply(&mut self.brakelight),
                     TURN_ON.apply(&mut self.headlight)
-                );
+                ).await;
             },
             Personality::Parked => {
                 warn!("Idle personality: Turning off safety lights");
-                join!(
+                embassy_futures::join::join(
                     TURN_OFF.apply(&mut self.brakelight),
                     TURN_OFF.apply(&mut self.headlight)
-                );
+                ).await;
             },
             Personality::Sleeping => {
                 warn!("Sleeping personality: Safety UI is going to sleep");

src/tasks/simulation.rs

@@ -1,56 +1,13 @@
-use core::cell::RefCell;
-use core::fmt::Formatter;
-
-use alloc::rc::Rc;
 use embassy_sync::channel::DynamicSender;
 use embassy_time::{Duration, Timer};
-use embedded_storage::{ReadStorage, Storage};
+use embedded_storage::ReadStorage;
 use esp_bootloader_esp_idf::partitions::PartitionTable;
 use esp_storage::FlashStorage;
 use nalgebra::{Vector2, Vector3};
 use log::*;
-use rmp::decode::{RmpRead, RmpReadErr, ValueReadError};
+use rmp::decode::ValueReadError;
 
-use crate::{Breaker, events::{Measurement, SensorSource, SensorState}, simdata::{AnnotationReading, EventRecord, EventStreamHeader, GPSReading, IMUReading, RmpData, SimDataError, StreamEvent, StreamHeader, StreamIndex, StreamType}};
+use crate::{Breaker, events::{Measurement, SensorSource, SensorState}, simdata::{AnnotationReading, EventRecord, EventStreamHeader, GPSReading, IMUReading, RmpData, SimDataError, StreamEvent, StreamHeader, StreamIndex, StreamType}, storage::{RangeReadError, RangeReader, SharedFlash}};
-
-#[derive(Debug)]
-pub struct SharedFlash<S> {
-    storage: Rc<RefCell<S>>
-}
-
-impl<S> Clone for SharedFlash<S> {
-    fn clone(&self) -> Self {
-        Self {
-            storage: Rc::clone(&self.storage)
-        }
-    }
-}
-
-impl<S> SharedFlash<S> {
-    pub fn new(storage: S) -> Self {
-        Self {
-            storage: Rc::new(RefCell::new(storage))
-        }
-    }
-}
-
-impl<S: Storage> Storage for SharedFlash<S> {
-    fn write(&mut self, offset: u32, bytes: &[u8]) -> Result<(), Self::Error> {
-        self.storage.borrow_mut().write(offset, bytes)
-    }
-}
-
-impl<S: ReadStorage> ReadStorage for SharedFlash<S> {
-    type Error = S::Error;
-
-    fn read(&mut self, offset: u32, bytes: &mut [u8]) -> Result<(), Self::Error> {
-        self.storage.borrow_mut().read(offset, bytes)
-    }
-
-    fn capacity(&self) -> usize {
-        self.storage.borrow().capacity()
-    }
-}
 
 pub struct SimDataTable<S> {
     reader: RangeReader<S>,
@@ -129,70 +86,6 @@ impl<S: ReadStorage + Clone + core::fmt::Debug> Iterator for SimDataTable<S> whe
     }
 }
 
-#[derive(Debug)]
-pub struct RangeReader<S> {
-    storage: S,
-    start: usize,
-    end: usize,
-    offset: usize
-}
-
-impl<S: ReadStorage> RangeReader<S> {
-    pub const fn new(storage: S, start: usize, end: usize) -> Self {
-        assert!(start <= end);
-        // TODO: Should add bounds checking since we will know the size of the chunk already
-        Self {
-            storage,
-            start,
-            end,
-            offset: 0
-        }
-    }
-
-    pub fn seek(&mut self, offset: usize) -> Result<(), RangeReadError<S::Error>> {
-        self.offset += offset;
-        if self.offset > self.end {
-            Err(RangeReadError::OutOfData)
-        } else {
-            Ok(())
-        }
-    }
-
-    pub fn subset(&self, size: usize) -> Result<Self, SimDataError<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 {
-            Err(SimDataError::EndOfStream)
-        } else {
-            Ok(Self {
-                storage: self.storage.clone(),
-                start: self.offset + self.start,
-                end: self.start + self.offset + size,
-                offset: 0
-            })
-        }
-    }
-}
-
-impl<S: ReadStorage> RmpRead for RangeReader<S> where S::Error: core::fmt::Debug + 'static {
-    type Error = RangeReadError<S::Error>;
-
-    fn read_exact_buf(&mut self, buf: &mut [u8]) -> Result<(), Self::Error> {
-        let pos = self.start + self.offset;
-        if pos > self.end {
-            Err(RangeReadError::OutOfData)
-        } else {
-            assert!(pos + buf.len() <= self.end);
-            match self.storage.read(pos as u32, buf) {
-                Ok(_) => {
-                    self.offset += buf.len();
-                    Ok(())
-                },
-                Err(err) => Err(RangeReadError::Storage(err))
-            }
-        }
-    }
-}
-
 pub struct SimDataReader<S> {
     reader: RangeReader<S>,
     srcid: SensorSource,
@@ -265,19 +158,6 @@ impl<S: ReadStorage> SimDataReader<S> where S::Error: core::fmt::Debug + 'static
     }
 }
 
-#[derive(Debug)]
-pub enum RangeReadError<E> {
-    OutOfData,
-    Storage(E)
-}
-impl<E: core::fmt::Debug + 'static> RmpReadErr for RangeReadError<E> {}
-
-impl<E> core::fmt::Display for RangeReadError<E> {
-    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
-        f.write_str("RmpErr")
-    }
-}
-
 #[embassy_executor::task(pool_size = 3)]
 pub async fn simulation_task(mut reader: SimDataReader<SharedFlash<FlashStorage>>, events: DynamicSender<'static, Measurement>) {
     warn!("Starting simulation for {:?}", reader.srcid());

src/tasks/ui.rs

@@ -99,12 +99,12 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
                 let panels = Animation::default().duration(Duration::from_millis(300)).to(128);
                 let bg = Animation::default().duration(Duration::from_millis(300)).to(32);
                 let motion = Animation::default().duration(Duration::from_secs(1)).to(0);
-                join!(
+                embassy_futures::join::join4(
                     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 => {
@@ -114,12 +114,12 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
                 let bg_fade = Animation::default().duration(Duration::from_millis(300)).to(128);
                 
                 // FIXME: The scenes shouldn't be touching the brake/headlights at all here. In fact, they should be dealt with in a whole separate task from the main UI, maybe running on the motion prediction executor
-                join!(
+                embassy_futures::join::join4(
                     fg_fade.apply(&mut self.tail),
                     fg_fade.apply(&mut self.panels),
                     bg_fade.apply(&mut self.background),
                     fg_fade.apply(&mut self.motion)
-                );
+                ).await;
             },
             Scene::Accelerating => {
                 self.motion.set_shader(Movement::default());

src/tasks/wifi.rs

@@ -1,100 +1,124 @@
 use alloc::string::ToString;
-use embassy_executor::Spawner;
+use embassy_sync::channel::DynamicSender;
 use embassy_sync::pubsub::DynSubscriber;
-use esp_radio::Controller;
+use embassy_time::{Duration, Instant, WithTimeout};
-use esp_radio::wifi::{ClientConfig, WifiDevice};
+use esp_hal::rng::Rng;
+use esp_radio::wifi::{ClientConfig, ScanConfig, WifiController, WifiDevice, WifiEvent};
 use log::*;
 use alloc::format;
 
 use embassy_net::dns::DnsSocket;
 use embassy_net::tcp::client::{TcpClient, TcpClientState};
-use embassy_net::{Config, StackResources};
+use embassy_net::Stack;
 use nalgebra::Vector2;
 use reqwless::client::{HttpClient, TlsConfig};
-use static_cell::StaticCell;
 
+use crate::ego::engine::gps_to_local_meters_haversine;
+use crate::events::{Measurement, SensorSource, SensorState};
 use crate::{backoff::Backoff, events::{Prediction}};
 
 #[embassy_executor::task]
-async fn net_task(mut runner: embassy_net::Runner<'static, WifiDevice<'static>>) {
+pub async fn net_task(mut runner: embassy_net::Runner<'static, WifiDevice<'static>>) {
-    info!("Network stack is running");
+    info!("Starting network stack");
     runner.run().await
 }
 
-static RESOURCES: StaticCell<StackResources<5>> = StaticCell::new();
-
-// TODO: Wifi task needs to know when there is data to upload, so it only connects when needed.
 #[embassy_executor::task]
-pub async fn wireless_task(mut predictions: DynSubscriber<'static, Prediction>, wifi_init: &'static mut Controller<'static>, wifi_device: esp_hal::peripherals::WIFI<'static>) {
+pub async fn wifi_connect_task(mut wifi: WifiController<'static>, stack: Stack<'static>, motion: DynamicSender<'static, Measurement>) {
-    let (mut wifi, interfaces) = esp_radio::wifi::new(wifi_init, wifi_device, esp_radio::wifi::Config::default())
-            .expect("Failed to initialize WIFI!");
     wifi.set_config(&esp_radio::wifi::ModeConfig::Client(
         ClientConfig::default()
-            .with_ssid("The Frequencey".to_string())
+            .with_ssid("The Frequency".to_string())
             .with_auth_method(esp_radio::wifi::AuthMethod::Wpa2Personal)
             .with_password("thepasswordkenneth".to_string())
     )).unwrap();
     wifi.set_mode(esp_radio::wifi::WifiMode::Sta).unwrap();
     wifi.set_power_saving(esp_radio::wifi::PowerSaveMode::Maximum).unwrap();
-    wifi.start_async().await.unwrap();
-
-    let device = interfaces.sta;
-    // TODO: Somehow grab a real random seed from main()
-    let seed = 0;
-
-    let config = Config::dhcpv4(Default::default());
-    let (stack, runner) = embassy_net::new(device, config, RESOURCES.init_with(|| { StackResources::new() }), seed as u64);
-    info!("Launching network task");
-    unsafe { Spawner::for_current_executor().await }.must_spawn(net_task(runner));
 
     loop {
         Backoff::from_secs(3).forever().attempt(async || {
             info!("Connecting to wifi...");
+            wifi.start_async().await.unwrap();
+            motion.send(Measurement::SensorHardwareStatus(SensorSource::Wifi, crate::events::SensorState::AcquiringFix)).await;
+            let networks = wifi.scan_with_config_async(ScanConfig::default().with_show_hidden(true)).await.unwrap();
+            for network in networks {
+                info!("wifi: {} @ {}db", network.ssid, network.signal_strength);
+            }
             match wifi.connect_async().await {
                 Ok(_) => Ok(()),
                 Err(e) => {
-                    error!("Wifi error: {e:?}");
+                    error!("Unable to connect to wifi: {e:?}");
+                    wifi.stop_async().await.unwrap();
                     Err(())
                 }
             }
         }).await.unwrap();
 
-        info!("Waiting for DHCP");
+        info!("Waiting for DHCP...");
-        stack.wait_config_up().await;
+        motion.send(Measurement::SensorHardwareStatus(SensorSource::Wifi, SensorState::Degraded)).await;
+        if stack.wait_config_up().with_timeout(Duration::from_secs(5)).await.is_ok() {
+            info!("Online!");
+            motion.send(Measurement::SensorHardwareStatus(SensorSource::Wifi, SensorState::Online)).await;
+            let ip_cfg = stack.config_v4().unwrap();
+            info!("ip={ip_cfg:?}");
+            wifi.wait_for_event(WifiEvent::ApStaDisconnected).await;
+            info!("Wifi disconnected!");
+        } else {
+            warn!("DHCP timed out after 5 seconds. Disconnecting wifi");
+            wifi.disconnect_async().await.unwrap();
+        }
+        warn!("Stopping wifi device");
+        wifi.stop_async().await.unwrap();
+        motion.send(Measurement::SensorHardwareStatus(SensorSource::Wifi, SensorState::Offline)).await;
+    }
+}
 
-        info!("Online!");
+// TODO: Wifi task needs to know when there is data to upload, so it only connects when needed.
-        let ip_cfg = stack.config_v4().unwrap();
+#[embassy_executor::task]
-        info!("ip={ip_cfg:?}");
+pub async fn http_telemetry_task(mut predictions: DynSubscriber<'static, Prediction>, stack: Stack<'static>) {
+    // TODO: should wait for wifi disconnect event somehow and use that to restart sending the wifi around
 
-        let mut rx_buf = [0; 4096];
+    let seed = Rng::new().random() as i32;
-        let mut tx_buf = [0; 4096];
+    let mut rx_buf = [0; 4096];
-        let dns = DnsSocket::new(stack);
+    let mut tx_buf = [0; 4096];
-        let tcp_state = TcpClientState::<1, 4096, 4096>::new();
+    let dns = DnsSocket::new(stack);
-        let tcp = TcpClient::new(stack, &tcp_state);
+    let tcp_state = TcpClientState::<1, 4096, 4096>::new();
-        let tls = TlsConfig::new(
+    let tcp = TcpClient::new(stack, &tcp_state);
-            seed as u64,
+    let tls = TlsConfig::new(
-            &mut rx_buf,
+        seed as u64,
-            &mut tx_buf,
+        &mut rx_buf,
-            reqwless::client::TlsVerify::None,
+        &mut tx_buf,
-        );
+        reqwless::client::TlsVerify::None,
+    );
 
-        let mut client = HttpClient::new_with_tls(&tcp, &dns, tls);
+    let mut client = HttpClient::new_with_tls(&tcp, &dns, tls);
 
-        loop {
+    // TODO: Only should upload a data point after some distance has occurred
-            if let Prediction::Location(coords) = predictions.next_message_pure().await {
+    let mut last_location = Vector2::default();
-                if let Err(e) = push_location(&mut client, coords).await {
+    let mut last_push = Instant::from_ticks(0);
-                    error!("HTTP error in publishing location: {e:?}");
+
-                    break
+    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 || {
+                        push_location(&mut client, coords, Instant::now().as_millis()).await
+                    }).await {
+                        warn!("Could not submit location! {e:?}");
+                    }
                 }
             }
         }
     }
 }
 
-async fn push_location(client: &mut HttpClient<'_, TcpClient<'_, 1, 4096, 4096>, DnsSocket<'_>>, location: Vector2<f64>) -> Result<(), reqwless::Error> {
+async fn push_location(client: &mut HttpClient<'_, TcpClient<'_, 1, 4096, 4096>, DnsSocket<'_>>, location: Vector2<f64>, timestamp: u64) -> Result<(), reqwless::Error> {
     let mut buffer = [0u8; 4096];
     let base = "https://nextcloud.malloc.hackerbots.net/nextcloud/index.php/apps/phonetrack/logGet/a062000067304e9dee6590f1b8f9e0db/renderbug";
-    let url = format!("{base}?lat={}&lon={}", location.y, location.x);
+    let url = format!("{base}?lon={}&lat={}&timestamp={}", location.y, location.x, timestamp);
     info!("Pushing to {url}");
     let mut http_req = client
         .request(
@@ -104,10 +128,9 @@ async fn push_location(client: &mut HttpClient<'_, TcpClient<'_, 1, 4096, 4096>,
         .await?;
     let response = http_req.send(&mut buffer).await?;
 
-    info!("Got response");
     let res = response.body().read_to_end().await?;
 
     let content = core::str::from_utf8(res).unwrap();
-    info!("{content}");
+    debug!("HTTP response: {content}");
     Ok(())
 }