From 3f651718a44f94c35fa2751fe86e4d7225b24bdf Mon Sep 17 00:00:00 2001 From: Victoria Fischer Date: Mon, 22 Dec 2025 15:55:06 +0100 Subject: [PATCH] tasks: motion: move the entire motion prediction engine into the second core, dedicating core 1 to hardware --- src/bin/main.rs | 19 ++++++++++++------- src/ego/engine.rs | 16 ++++++++-------- src/events.rs | 12 ++++++------ src/tasks/motion.rs | 8 ++++---- src/tasks/predict.rs | 3 +++ 5 files changed, 33 insertions(+), 25 deletions(-) diff --git a/src/bin/main.rs b/src/bin/main.rs index eb2d24f..73c9f1f 100644 --- a/src/bin/main.rs +++ b/src/bin/main.rs @@ -20,7 +20,7 @@ use esp_hal::{ use log::*; use renderbug_embassy::{graphics::display::DisplayControls, logging::RenderbugLogger, tasks::{oled::{OledUI, OledUiSurfacePool, oled_ui}, safetyui::{SafetyUi, safety_ui_main}, ui::UiSurfacePool}}; -use renderbug_embassy::events::BusGarage; +use renderbug_embassy::events::{BusGarage, Measurement}; use static_cell::StaticCell; use esp_backtrace as _; use esp_rtos::embassy::{Executor, InterruptExecutor}; @@ -31,6 +31,8 @@ use renderbug_embassy::tasks::{ ui::{Ui, ui_main} }; +use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; +use embassy_sync::channel::Channel; extern crate alloc; @@ -68,7 +70,8 @@ async fn main(spawner: Spawner) { ui_wdt.set_timeout(esp_hal::timer::timg::MwdtStage::Stage0, esp_hal::time::Duration::from_secs(10)); ui_wdt.enable(); - let garage = BUS_GARAGE.init_with(|| { Default::default() }); + static MOTION_BUS: StaticCell > = StaticCell::new(); + let motion_bus = MOTION_BUS.init_with(|| { Channel::new() }); info!("Setting up rendering pipeline"); let mut surfaces = UiSurfacePool::default(); @@ -107,9 +110,9 @@ async fn main(spawner: Spawner) { let i2c = I2c::new(peripherals.I2C1, Config::default()).unwrap().with_scl(scl).with_sda(sda).into_async(); let i2c_bus = I2C_BUS.init(Mutex::new(i2c)); #[cfg(feature="mpu")] - spawner.must_spawn(renderbug_embassy::tasks::mpu::mpu_task(garage.motion.dyn_sender(), I2cDevice::new(i2c_bus))); + spawner.must_spawn(renderbug_embassy::tasks::mpu::mpu_task(motion_bus.dyn_sender(), I2cDevice::new(i2c_bus))); #[cfg(feature="gps")] - spawner.must_spawn(renderbug_embassy::tasks::gps::gps_task(garage.motion.dyn_sender(), I2cDevice::new(i2c_bus))); + spawner.must_spawn(renderbug_embassy::tasks::gps::gps_task(motion_bus.dyn_sender(), I2cDevice::new(i2c_bus))); } #[cfg(feature="oled")] @@ -141,9 +144,6 @@ async fn main(spawner: Spawner) { } } - info!("Launching motion engine"); - spawner.must_spawn(motion_task(garage.motion.dyn_receiver(), garage.notify.dyn_publisher().unwrap(), garage.predict.dyn_sender())); - #[cfg(feature="radio")] let wifi_init = { info!("Configuring wifi"); @@ -156,6 +156,11 @@ async fn main(spawner: Spawner) { 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() }); exec.run(|spawner| { + let garage = BUS_GARAGE.init_with(|| { Default::default() }); + + info!("Launching motion engine"); + spawner.must_spawn(motion_task(motion_bus.dyn_receiver(), garage.predict.dyn_sender())); + info!("Launching Safety UI"); spawner.must_spawn(safety_ui_main(garage.notify.dyn_subscriber().unwrap(), safety_ui)); info!("Launching UI"); diff --git a/src/ego/engine.rs b/src/ego/engine.rs index fac515d..94f4624 100644 --- a/src/ego/engine.rs +++ b/src/ego/engine.rs @@ -110,25 +110,25 @@ impl BikeStates { } } - pub async fn commit(&mut self, predictions: &DynamicSender<'static, Prediction>, notifications: &DynPublisher<'static, Notification>) { + pub async fn commit(&mut self, predictions: &DynamicSender<'static, Prediction>) { if let Some(true) = self.acquiring_data.read_tripped() { - notifications.publish(Notification::SensorStatus(SensorSource::ForwardsReference, SensorState::AcquiringFix)).await; - notifications.publish(Notification::SensorStatus(SensorSource::GravityReference, SensorState::AcquiringFix)).await; - notifications.publish(Notification::SensorStatus(SensorSource::Location, SensorState::AcquiringFix)).await; + predictions.send(Prediction::SensorStatus(SensorSource::ForwardsReference, SensorState::AcquiringFix)).await; + predictions.send(Prediction::SensorStatus(SensorSource::GravityReference, SensorState::AcquiringFix)).await; + predictions.send(Prediction::SensorStatus(SensorSource::Location, SensorState::AcquiringFix)).await; } if let Some(true) = self.has_down.read_tripped() { - notifications.publish(Notification::SensorStatus(SensorSource::GravityReference, SensorState::Online)).await + predictions.send(Prediction::SensorStatus(SensorSource::GravityReference, SensorState::Online)).await } if let Some(true) = self.has_forwards.read_tripped() { - notifications.publish(Notification::SensorStatus(SensorSource::ForwardsReference, SensorState::Online)).await + predictions.send(Prediction::SensorStatus(SensorSource::ForwardsReference, SensorState::Online)).await } match self.has_gps_fix.read_tripped() { None => (), - Some(true) => notifications.publish(Notification::SensorStatus(SensorSource::Location, SensorState::Online)).await, - Some(false) => notifications.publish(Notification::SensorStatus(SensorSource::Location, SensorState::Degraded)).await, + Some(true) => predictions.send(Prediction::SensorStatus(SensorSource::Location, SensorState::Online)).await, + Some(false) => predictions.send(Prediction::SensorStatus(SensorSource::Location, SensorState::Degraded)).await, } let est = self.kf.x; diff --git a/src/events.rs b/src/events.rs index e6051c0..ffde1f1 100644 --- a/src/events.rs +++ b/src/events.rs @@ -52,7 +52,9 @@ pub enum Prediction { Motion(MotionState), Velocity(f32), Location(Vector2), - WakeRequested + WakeRequested, + // States of external connections to the world + SensorStatus(SensorSource, SensorState), } #[derive(Clone, Copy, Debug)] @@ -114,16 +116,14 @@ impl TryFrom for SensorSource { #[derive(Debug)] pub struct BusGarage { - pub motion: Channel, - pub notify: PubSubChannel, - pub predict: Channel, - pub telemetry: PubSubChannel + pub notify: PubSubChannel, + pub predict: Channel, + pub telemetry: PubSubChannel } impl Default for BusGarage { fn default() -> Self { Self { - motion: Channel::new(), notify: PubSubChannel::new(), predict: Channel::new(), telemetry: PubSubChannel::new() diff --git a/src/tasks/motion.rs b/src/tasks/motion.rs index c0f85d1..e5e7021 100644 --- a/src/tasks/motion.rs +++ b/src/tasks/motion.rs @@ -4,7 +4,7 @@ use log::*; use crate::{ego::engine::BikeStates, events::{Measurement, Notification, Prediction, SensorSource, SensorState}}; #[embassy_executor::task] -pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, ui_sink: DynPublisher<'static, Notification>, prediction_sink: DynamicSender<'static, Prediction>) { +pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_sink: DynamicSender<'static, Prediction>) { let mut states = BikeStates::default(); loop { @@ -14,11 +14,11 @@ pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, ui_sink: Dy match next_measurement { Measurement::IMU { accel, gyro } => { states.insert_imu(accel, gyro); - states.commit(&prediction_sink, &ui_sink).await; + states.commit(&prediction_sink).await; }, Measurement::GPS(Some(gps_pos)) => { states.insert_gps(gps_pos); - states.commit(&prediction_sink, &ui_sink).await; + states.commit(&prediction_sink).await; }, Measurement::GPS(None) => { states.has_gps_fix.set(false); @@ -26,7 +26,7 @@ pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, ui_sink: Dy // FIXME: This needs harmonized with the automatic data timeout from above, somehow? Measurement::SensorHardwareStatus(source, state) => { warn!("Sensor {source:?} reports {state:?}!"); - ui_sink.publish(Notification::SensorStatus(source, state)).await; + prediction_sink.send(Prediction::SensorStatus(source, state)).await; }, Measurement::SimulationProgress(source, duration, _pct) => debug!("{source:?} simulation time: {}", duration.as_secs()), Measurement::Annotation => () diff --git a/src/tasks/predict.rs b/src/tasks/predict.rs index ae9352c..53a1b80 100644 --- a/src/tasks/predict.rs +++ b/src/tasks/predict.rs @@ -87,6 +87,9 @@ pub async fn prediction_task(prediction_src: DynamicReceiver<'static, Prediction stationary = true } } + }, + Prediction::SensorStatus(src, status) => { + notify.publish(Notification::SensorStatus(src, status)).await; } } }