tdfischer/renderbug-bike
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Victoria Fischer
2025-12-22 15:55:06 +01:00
parent 73b037f0c2
commit 3f651718a4
5 changed files with 33 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src/bin/main.rs
View File

@@ -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");

16
src/ego/engine.rs
View File

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

12
src/events.rs
View File

@@ -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<CriticalSectionRawMutex, Notification, 5, 2, 4>,
pub predict: Channel<CriticalSectionRawMutex, Prediction, 15>,
pub telemetry: PubSubChannel<CriticalSectionRawMutex, Telemetry, 15, 2, 4>
pub notify: PubSubChannel<NoopRawMutex, Notification, 5, 2, 4>,
pub predict: Channel<NoopRawMutex, Prediction, 15>,
pub telemetry: PubSubChannel<NoopRawMutex, Telemetry, 15, 2, 4>
}
impl Default for BusGarage {
fn default() -> Self {
Self {
motion: Channel::new(),
notify: PubSubChannel::new(),
predict: Channel::new(),
telemetry: PubSubChannel::new()

8
src/tasks/motion.rs
View File

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

3
src/tasks/predict.rs
View File

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