tasks: motion: move the entire motion prediction engine into the second core, dedicating core 1 to hardware

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<Channel<CriticalSectionRawMutex,Measurement,5> > = 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");

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;
+            predictions.send(Prediction::SensorStatus(SensorSource::ForwardsReference, SensorState::AcquiringFix)).await;
-            notifications.publish(Notification::SensorStatus(SensorSource::GravityReference, SensorState::AcquiringFix)).await;
+            predictions.send(Prediction::SensorStatus(SensorSource::GravityReference, SensorState::AcquiringFix)).await;
-            notifications.publish(Notification::SensorStatus(SensorSource::Location, 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(true) => predictions.send(Prediction::SensorStatus(SensorSource::Location, SensorState::Online)).await,
-            Some(false) => notifications.publish(Notification::SensorStatus(SensorSource::Location, SensorState::Degraded)).await,
+            Some(false) => predictions.send(Prediction::SensorStatus(SensorSource::Location, SensorState::Degraded)).await,
         }
 
         let est = self.kf.x;

src/events.rs

@@ -52,7 +52,9 @@ pub enum Prediction {
     Motion(MotionState),
     Velocity(f32),
     Location(Vector2<f64>),
-    WakeRequested
+    WakeRequested,
+    // States of external connections to the world
+    SensorStatus(SensorSource, SensorState),
 }
 
 #[derive(Clone, Copy, Debug)]
@@ -114,16 +116,14 @@ impl TryFrom<i8> for SensorSource {
 
 #[derive(Debug)]
 pub struct BusGarage {
-    pub motion: Channel<NoopRawMutex, Measurement, 5>,
+    pub notify: PubSubChannel<NoopRawMutex, Notification, 5, 2, 4>,
-    pub notify: PubSubChannel<CriticalSectionRawMutex, Notification, 5, 2, 4>,
+    pub predict: Channel<NoopRawMutex, Prediction, 15>,
-    pub predict: Channel<CriticalSectionRawMutex, Prediction, 15>,
+    pub telemetry: PubSubChannel<NoopRawMutex, Telemetry, 15, 2, 4>
-    pub telemetry: PubSubChannel<CriticalSectionRawMutex, Telemetry, 15, 2, 4>
 }
 
 impl Default for BusGarage {
     fn default() -> Self {
         Self {
-            motion: Channel::new(),
             notify: PubSubChannel::new(),
             predict: Channel::new(),
             telemetry: PubSubChannel::new()

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 => ()

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;
                 }
             }
         }