diff --git a/src/bin/main.rs b/src/bin/main.rs index c0a60b3..2e6fa8e 100644 --- a/src/bin/main.rs +++ b/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 > = StaticCell::new(); let motion_bus = MOTION_BUS.init_with(|| { Channel::new() }); + static RECORDING_BUS: StaticCell > = 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>) { } #[embassy_executor::task] -async fn print_telemetry(mut events: DynSubscriber<'static, Prediction>) { +async fn record_telemetry(firehose: DynamicReceiver<'static, Measurement>, mut storage: SimDataRecorder>) { + 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}"); + }, + _ => () + } } } \ No newline at end of file diff --git a/src/tasks/motion.rs b/src/tasks/motion.rs index e9859f9..d83e262 100644 --- a/src/tasks/motion.rs +++ b/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); } } \ No newline at end of file