tasks: motion: implement a sensor recording layer

src/bin/main.rs

@@ -11,21 +11,21 @@ use core::{num::{self, Wrapping}, ptr::addr_of_mut};
 
 use alloc::{string::String, sync::Arc};
 use embassy_executor::Spawner;
-use embassy_time::{Instant, Timer};
+use embassy_time::{Duration, Instant, Timer, WithTimeout};
 
-use esp_hal::{gpio::{Output, OutputConfig, Pin}, time::Rate, xtensa_lx::debug_break};
+use enum_map::EnumMap;
+use esp_hal::{gpio::{Output, OutputConfig, Pin}, time::Rate};
 use esp_hal::{
     clock::CpuClock, system::{AppCoreGuard, CpuControl, Stack}, timer::{systimer::SystemTimer, timg::{TimerGroup, Wdt}}
 };
 
 use embassy_sync::{
-    pubsub::PubSubChannel,
-    blocking_mutex::raw::NoopRawMutex
+    blocking_mutex::raw::NoopRawMutex, channel::DynamicReceiver, pubsub::PubSubChannel
 };
-use static_cell::ConstStaticCell;
+use esp_storage::FlashStorage;
 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;
+use renderbug_bike::{events::{Prediction, SensorSource, SensorState}, graphics::display::DisplayControls, logging::RenderbugLogger, simdata::IMUReading, storage::{SharedFlash, SimDataRecorder}, tasks::{oled::{OledUI, OledUiSurfacePool, oled_ui}, safetyui::{SafetyUi, safety_ui_main}, ui::UiSurfacePool}, tracing::Tracer};
+use renderbug_bike::events::Measurement;
 use static_cell::StaticCell;
 use esp_backtrace as _;
 use esp_rtos::embassy::{Executor, InterruptExecutor};
@@ -63,6 +63,9 @@ async fn main(spawner: Spawner) {
     static MOTION_BUS: StaticCell<Channel<CriticalSectionRawMutex,Measurement,5> > = StaticCell::new();
     let motion_bus = MOTION_BUS.init_with(|| { Channel::new() });
 
+    static RECORDING_BUS: StaticCell<PubSubChannel<CriticalSectionRawMutex,Measurement,1, 1, 1> > = StaticCell::new();
+    let recording_bus = RECORDING_BUS.init_with(|| { PubSubChannel::new() });
+
     info!("Setting up rendering pipeline");
     let mut surfaces = UiSurfacePool::default();
     let ui = Ui::new(&mut surfaces);
@@ -218,7 +221,7 @@ async fn main(spawner: Spawner) {
             spawner.must_spawn(wdt_task(ui_wdt));
         }
 
-        spawner.must_spawn(print_telemetry(predictions.dyn_subscriber().unwrap()));
+        spawner.must_spawn(print_sensor_status(predictions.dyn_subscriber().unwrap()));
 
         info!("System is ready in {}ms", Instant::now().as_millis());
     };
@@ -253,12 +256,77 @@ async fn wdt_task(mut wdt: Wdt<esp_hal::peripherals::TIMG1<'static>>) {
 }
 
 #[embassy_executor::task]
-async fn print_telemetry(mut events: DynSubscriber<'static, Prediction>) {
+async fn record_telemetry(firehose: DynamicReceiver<'static, Measurement>, mut storage: SimDataRecorder<SharedFlash<FlashStorage>>) {
+    loop {
+        match firehose.receive().await {
+            Measurement::IMU { accel, gyro } => {
+                let reading = IMUReading {
+                    accel_x: accel.x as f64,
+                    accel_y: accel.y as f64,
+                    accel_z: accel.z as f64,
+                    gyro_x: gyro.x as f64,
+                    gyro_y: gyro.y as f64,
+                    gyro_z: gyro.z as f64
+                };
+                storage.write_next(reading).unwrap();
+                info!("Wrote IMU to flash");
+            },
+            _ => ()
+        }
+    }
+}
+
+#[embassy_executor::task]
+async fn print_sensor_readings(mut events: DynSubscriber<'static, Measurement>) {
+    loop {
+        match events.next_message_pure().await {
+            Measurement::IMU { accel, gyro } => {
+                esp_println::println!("accel=({},{},{}) gyro=({},{},{})", accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z);
+            },
+            Measurement::GPS(gps) => {
+                esp_println::println!("gps={gps:?}");
+            },
+            _ => ()
+        }
+    }
+}
+
+#[embassy_executor::task]
+async fn print_sensor_status(mut events: DynSubscriber<'static, Prediction>) {
+
     info!("telemetry ready");
     let mut num_events = Wrapping(0usize);
     loop {
-        let next = events.next_message_pure().await;
-        trace!("idx={} predict={next:?}", num_events.0);
-        num_events += 1;        
+        let next = events.next_message_pure().with_timeout(Duration::from_secs(5)).await;
+        match next {
+            Ok(Prediction::SensorStatus(sensor, status)) => {
+                sensor_states[sensor] = status;
+                let mut report_str = String::new();
+                for (sensor, state) in &sensor_states {
+                    let state_icon = match state {
+                        SensorState::AcquiringFix => "?",
+                        SensorState::Degraded => "-",
+                        SensorState::Offline => "X",
+                        SensorState::Online => "O"
+                    };
+                    report_str += alloc::format!("{sensor:?}={state_icon} ").as_str();
+                }
+                info!("{report_str}");
+            },
+            Err(_) => {
+                let mut report_str = String::new();
+                for (sensor, state) in &sensor_states {
+                    let state_icon = match state {
+                        SensorState::AcquiringFix => "?",
+                        SensorState::Degraded => "-",
+                        SensorState::Offline => "X",
+                        SensorState::Online => "O"
+                    };
+                    report_str += alloc::format!("{sensor:?}={state_icon} ").as_str();
+                }
+                info!("{report_str}");
+            },
+            _ => ()
+        }   
     }
 }

src/tasks/motion.rs

@@ -7,7 +7,7 @@ use crate::{ego::engine::BikeStates, events::{Measurement, Prediction}};
 const TIMEOUT: Duration = Duration::from_millis(3);
 
 #[embassy_executor::task]
-pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_sink: DynPublisher<'static, Prediction>) {
+pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_sink: DynPublisher<'static, Prediction>, recording_sink: DynPublisher<'static, Measurement>) {
     let mut states = BikeStates::default();
 
     loop {
@@ -34,5 +34,6 @@ pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_
             Measurement::SimulationProgress(source, duration, pct) => debug!("{source:?} simulation time: {} {} / 255", duration.as_secs(), pct),
             Measurement::Annotation => ()
         }
+        let _ = recording_sink.try_publish(next_measurement);
     }
 }