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

events: rewrite the eventing system to reduce mutex usage to just the measurement bus

Browse Source
This commit is contained in:
Victoria Fischer
2025-12-24 09:11:16 +01:00
parent 36f232f43c
commit 046406291a
11 changed files with 185 additions and 284 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/bin/main.rs
View File

@@ -159,19 +159,19 @@ async fn main(spawner: Spawner) {
let garage = BUS_GARAGE.init_with(|| { Default::default() }); let garage = BUS_GARAGE.init_with(|| { Default::default() });
info!("Launching motion engine"); info!("Launching motion engine");
spawner.must_spawn(motion_task(motion_bus.dyn_receiver(), garage.predict.dyn_sender())); spawner.must_spawn(motion_task(motion_bus.dyn_receiver(), garage.predict.dyn_publisher().unwrap()));
info!("Launching Safety UI"); info!("Launching Safety UI");
spawner.must_spawn(safety_ui_main(garage.notify.dyn_subscriber().unwrap(), safety_ui)); spawner.must_spawn(safety_ui_main(garage.predict.dyn_subscriber().unwrap(), safety_ui));
info!("Launching UI"); info!("Launching UI");
spawner.must_spawn(ui_main(garage.notify.dyn_subscriber().unwrap(), garage.telemetry.dyn_publisher().unwrap(), ui)); spawner.must_spawn(ui_main(garage.predict.dyn_subscriber().unwrap(), ui));
info!("Launching OLED UI"); info!("Launching OLED UI");
spawner.must_spawn(oled_ui(garage.telemetry.dyn_subscriber().unwrap(), oledui)); spawner.must_spawn(oled_ui(garage.predict.dyn_subscriber().unwrap(), oledui));
#[cfg(feature="radio")] #[cfg(feature="radio")]
{ {
info!("Launching networking stack"); info!("Launching networking stack");
spawner.must_spawn(renderbug_embassy::tasks::wifi::wireless_task(garage.telemetry.dyn_subscriber().unwrap(), wifi_init, peripherals.WIFI)); spawner.must_spawn(renderbug_embassy::tasks::wifi::wireless_task(garage.predict.dyn_subscriber().unwrap(), wifi_init, peripherals.WIFI));
} }
#[cfg(feature="demo")] #[cfg(feature="demo")]
@@ -179,11 +179,6 @@ async fn main(spawner: Spawner) {
warn!("Launching with demo sequencer"); warn!("Launching with demo sequencer");
spawner.must_spawn(renderbug_embassy::tasks::demo::demo_task(garage.notify.dyn_publisher().unwrap())); spawner.must_spawn(renderbug_embassy::tasks::demo::demo_task(garage.notify.dyn_publisher().unwrap()));
} }
#[cfg(not(feature="demo"))]
{
info!("Launching prediction engine");
spawner.must_spawn(renderbug_embassy::tasks::predict::prediction_task(garage.predict.dyn_receiver(), garage.notify.dyn_publisher().unwrap(), garage.telemetry.dyn_publisher().unwrap()));
}
info!("Launching core 2 watchdog"); info!("Launching core 2 watchdog");
spawner.must_spawn(wdt_task(ui_wdt)); spawner.must_spawn(wdt_task(ui_wdt));

99
src/ego/engine.rs
View File

@@ -5,7 +5,7 @@ use log::*;
use core::fmt::Debug; use core::fmt::Debug;
use crate::{Breaker, CircularBuffer, ego::{heading::HeadingEstimator, kalman::Ekf2D, orientation::OrientationEstimator}, events::{Notification, Prediction, SensorSource, SensorState}, idle::IdleClock}; use crate::{Breaker, CircularBuffer, ego::{heading::HeadingEstimator, kalman::Ekf2D, orientation::OrientationEstimator}, events::{Personality, Prediction, SensorSource, SensorState}, idle::IdleClock};
#[derive(PartialEq, Debug, Default, Clone, Copy)] #[derive(PartialEq, Debug, Default, Clone, Copy)]
pub enum MotionState { pub enum MotionState {
@@ -37,8 +37,10 @@ pub struct BikeStates {
predicted_velocity: Breaker<f32>, predicted_velocity: Breaker<f32>,
reported_velocity: Breaker<f32>, reported_velocity: Breaker<f32>,
predicted_location: Breaker<Vector2<f64>>, predicted_location: Breaker<Vector2<f64>>,
wake_requested: Breaker<bool>, steady_timer: IdleClock,
steady_timer: IdleClock
parking_timer: IdleClock,
sleep_timer: IdleClock,
} }
impl Debug for BikeStates { impl Debug for BikeStates {
@@ -50,7 +52,9 @@ impl Debug for BikeStates {
.field("motion_state", &self.motion_state) .field("motion_state", &self.motion_state)
.field("predicted_location", &self.predicted_location) .field("predicted_location", &self.predicted_location)
.field("predicted_velocity", &self.predicted_velocity) .field("predicted_velocity", &self.predicted_velocity)
.field("wake_requested", &self.wake_requested) .field("steady_timer", &self.steady_timer)
.field("parking_timer", &self.parking_timer)
.field("sleep_timer", &self.sleep_timer)
.finish() .finish()
} }
} }
@@ -95,7 +99,7 @@ impl BikeStates {
let heading_rotation = Rotation3::from_axis_angle(&Vector3::z_axis(), self.heading.heading()); let heading_rotation = Rotation3::from_axis_angle(&Vector3::z_axis(), self.heading.heading());
let enu_rotated = heading_rotation * body_accel; let enu_rotated = heading_rotation * body_accel;
if accel.xy().magnitude() >= 0.8 { if body_accel.xy().magnitude() >= 0.8 {
self.kf.predict(enu_rotated.xy(), body_gyro.z, dt); self.kf.predict(enu_rotated.xy(), body_gyro.z, dt);
} else { } else {
// Otherwise, we are standing stationary and should insert accel=0 data into the model // Otherwise, we are standing stationary and should insert accel=0 data into the model
@@ -110,25 +114,27 @@ impl BikeStates {
} }
} }
pub async fn commit(&mut self, predictions: &DynamicSender<'static, Prediction>) { pub async fn commit(&mut self, predictions: &DynPublisher<'static, Prediction>) {
let last_motion = self.motion_state.value;
if let Some(true) = self.acquiring_data.read_tripped() { if let Some(true) = self.acquiring_data.read_tripped() {
predictions.send(Prediction::SensorStatus(SensorSource::ForwardsReference, SensorState::AcquiringFix)).await; predictions.publish(Prediction::SensorStatus(SensorSource::ForwardsReference, SensorState::AcquiringFix)).await;
predictions.send(Prediction::SensorStatus(SensorSource::GravityReference, SensorState::AcquiringFix)).await; predictions.publish(Prediction::SensorStatus(SensorSource::GravityReference, SensorState::AcquiringFix)).await;
predictions.send(Prediction::SensorStatus(SensorSource::Location, SensorState::AcquiringFix)).await; predictions.publish(Prediction::SensorStatus(SensorSource::Location, SensorState::AcquiringFix)).await;
} }
if let Some(true) = self.has_down.read_tripped() { if let Some(true) = self.has_down.read_tripped() {
predictions.send(Prediction::SensorStatus(SensorSource::GravityReference, SensorState::Online)).await predictions.publish(Prediction::SensorStatus(SensorSource::GravityReference, SensorState::Online)).await
} }
if let Some(true) = self.has_forwards.read_tripped() { if let Some(true) = self.has_forwards.read_tripped() {
predictions.send(Prediction::SensorStatus(SensorSource::ForwardsReference, SensorState::Online)).await predictions.publish(Prediction::SensorStatus(SensorSource::ForwardsReference, SensorState::Online)).await
} }
match self.has_gps_fix.read_tripped() { match self.has_gps_fix.read_tripped() {
None => (), None => (),
Some(true) => predictions.send(Prediction::SensorStatus(SensorSource::Location, SensorState::Online)).await, Some(true) => predictions.publish(Prediction::SensorStatus(SensorSource::Location, SensorState::Online)).await,
Some(false) => predictions.send(Prediction::SensorStatus(SensorSource::Location, SensorState::Degraded)).await, Some(false) => predictions.publish(Prediction::SensorStatus(SensorSource::Location, SensorState::Degraded)).await,
} }
let est = self.kf.x; let est = self.kf.x;
@@ -140,29 +146,43 @@ impl BikeStates {
if let Some(pos) = position { if let Some(pos) = position {
self.predicted_location.set(pos); self.predicted_location.set(pos);
if let Some(pos) = self.predicted_location.read_tripped() { if let Some(pos) = self.predicted_location.read_tripped() {
predictions.send(Prediction::Location(pos)).await; predictions.publish(Prediction::Location(pos)).await;
} }
} }
// If we have a new velocity, update the acceleration status // If we have a new predicted velocity, update the acceleration status
self.predicted_velocity.set(velocity.norm()); self.predicted_velocity.set(velocity.norm());
if let Some(v) = self.predicted_velocity.read_tripped() { if let Some(current_prediction) = self.predicted_velocity.read_tripped() {
self.wake_requested.set(true); // Add the prediction into the speedometer model
if self.wake_requested.read_tripped().is_some() { self.speedo.insert(current_prediction);
predictions.send(Prediction::WakeRequested).await; // If the model has enough samples to report useful data, we can start analyzing the motion trends
}
self.speedo.insert(v);
if self.speedo.is_filled() { if self.speedo.is_filled() {
let threshold = 1.0; let threshold = 1.0;
// Calculate if the velocity is increasing, decreasing, or mostly the same
let trend = self.speedo.data().windows(2).map(|n| { let trend = self.speedo.data().windows(2).map(|n| {
n[1] - n[0] n[1] - n[0]
}).sum::<f32>(); }).sum::<f32>();
// Also grab the average velocity of the last few sample periods
let mean = self.speedo.mean(); let mean = self.speedo.mean();
// Reported velocity is kept only to the first decimal // Reported velocity is kept only to the first decimal, so we aren't spamming the system with floating point noise
self.reported_velocity.set((mean *10.0).round() / 10.0); self.reported_velocity.set((mean * 10.0).trunc() / 10.0);
if let Some(v) = self.reported_velocity.read_tripped() { if let Some(reported) = self.reported_velocity.read_tripped() {
predictions.send(Prediction::Velocity(v)).await; predictions.publish(Prediction::Velocity(reported)).await;
}
// We only want to wake up from sleep if our current velocity is obviously intentional, eg not a quick bump
if mean > 0.5 {
if self.sleep_timer.wake() {
warn!("Waking from sleep into idle mode");
predictions.publish(Prediction::SetPersonality(Personality::Waking)).await;
predictions.publish(Prediction::SetPersonality(Personality::Parked)).await;
}
// Here, we additionally release the parking brake if we are currently parked and reading some kind of significant movement
if self.parking_timer.wake() {
warn!("Disengaging parking brake");
predictions.publish(Prediction::SetPersonality(Personality::Active)).await;
}
} }
// If the total slope is more upwards than not, we are accelerating. // If the total slope is more upwards than not, we are accelerating.
@@ -175,19 +195,31 @@ impl BikeStates {
} else if self.steady_timer.check() && mean > 1.0 { } else if self.steady_timer.check() && mean > 1.0 {
// If we haven't changed our acceleration for a while, and we still have speed, we are moving at a steady pace // If we haven't changed our acceleration for a while, and we still have speed, we are moving at a steady pace
self.motion_state.set(MotionState::Steady); self.motion_state.set(MotionState::Steady);
} else if v <= 1.0 && mean <= 1.0 { } else if current_prediction <= 1.0 && mean <= 1.0 {
// If the average and instantaneous speed is rather low, we are probably stationary! // If the average and instantaneous speed is rather low, we are probably stationary!
self.motion_state.set(MotionState::Stationary); self.motion_state.set(MotionState::Stationary);
} }
}
}
// And if the motion status has changed, send it out // And if the motion status has changed, send it out
if let Some(state) = self.motion_state.read_tripped() { if let Some(state) = self.motion_state.read_tripped() {
debug!("state={state:?} trend={trend} mean={mean} v={v}"); //debug!("state={state:?} trend={trend} mean={mean} v={v}");
predictions.send(Prediction::Motion(state)).await; //if state != MotionState::Stationary {
// warn!("Active due to motion");
// predictions.publish(Prediction::SetPersonality(Personality::Active)).await;
//}
predictions.publish(Prediction::Motion { prev: last_motion, next: state }).await;
} else if self.motion_state.value == MotionState::Stationary {
// Finally, if we are stationary, check our parking and sleep timers
if self.parking_timer.check() {
warn!("Engaging parking brake");
predictions.publish(Prediction::SetPersonality(Personality::Parked)).await;
} }
if self.sleep_timer.check() {
warn!("Sleeping!");
predictions.publish(Prediction::SetPersonality(Personality::Sleeping)).await;
} }
} else {
self.wake_requested.set(false);
} }
} }
} }
@@ -210,8 +242,9 @@ impl Default for BikeStates {
predicted_location: Default::default(), predicted_location: Default::default(),
predicted_velocity: Default::default(), predicted_velocity: Default::default(),
reported_velocity: Default::default(), reported_velocity: Default::default(),
wake_requested: Default::default(), acquiring_data: Default::default(),
acquiring_data: Default::default() parking_timer: IdleClock::new(Duration::from_secs(10)),
sleep_timer: IdleClock::new(Duration::from_secs(30))
} }
} }
} }

30
src/events.rs
View File

@@ -1,11 +1,11 @@
use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::Channel, pubsub::PubSubChannel}; use embassy_sync::{blocking_mutex::raw::NoopRawMutex, pubsub::PubSubChannel};
use embassy_time::Duration; use embassy_time::Duration;
use enum_map::Enum; use enum_map::Enum;
use enumset::EnumSetType; use enumset::EnumSetType;
use nalgebra::{Vector2, Vector3}; use nalgebra::{Vector2, Vector3};
use crate::{graphics::display::DisplayControls, ego::engine::MotionState}; use crate::ego::engine::MotionState;
#[derive(Clone, Copy, Default, Debug)] #[derive(Clone, Copy, Default, Debug)]
pub enum Scene { pub enum Scene {
@@ -47,14 +47,24 @@ pub enum Measurement {
Annotation Annotation
} }
#[derive(Default, Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)]
pub enum Personality {
Sleeping, // System should be using as low power as possible, displays off, etc
#[default]
Waking, // System is resuming from sleep, or it is the first boot. A transient state that quickly turns into Active.
Parked, // System should be acting like an art piece with some idle animations, maybe it can search for some wifi/bluetooth
Active // System is almost likely on the road and must provide lighting
}
#[derive(Clone, Copy, Debug)] #[derive(Clone, Copy, Debug)]
pub enum Prediction { pub enum Prediction {
Motion(MotionState), Motion { prev: MotionState, next: MotionState },
Velocity(f32), Velocity(f32),
Location(Vector2<f64>), Location(Vector2<f64>),
WakeRequested,
// States of external connections to the world // States of external connections to the world
SensorStatus(SensorSource, SensorState), SensorStatus(SensorSource, SensorState),
// The system should enter into one of the four personalities
SetPersonality(Personality)
} }
#[derive(Clone, Copy, Debug)] #[derive(Clone, Copy, Debug)]
@@ -77,12 +87,6 @@ pub enum Notification {
Beat, Beat,
} }
#[derive(Clone, Copy, Debug)]
pub enum Telemetry {
Notification(Notification),
Prediction(Prediction),
}
// GPS data = 2, motion data = 1 // GPS data = 2, motion data = 1
#[derive(Debug, EnumSetType, Enum)] #[derive(Debug, EnumSetType, Enum)]
pub enum SensorSource { pub enum SensorSource {
@@ -117,16 +121,14 @@ impl TryFrom<i8> for SensorSource {
#[derive(Debug)] #[derive(Debug)]
pub struct BusGarage { pub struct BusGarage {
pub notify: PubSubChannel<NoopRawMutex, Notification, 5, 2, 4>, pub notify: PubSubChannel<NoopRawMutex, Notification, 5, 2, 4>,
pub predict: Channel<NoopRawMutex, Prediction, 15>, pub predict: PubSubChannel<NoopRawMutex, Prediction, 15, 3, 3>,
pub telemetry: PubSubChannel<NoopRawMutex, Telemetry, 15, 2, 4>
} }
impl Default for BusGarage { impl Default for BusGarage {
fn default() -> Self { fn default() -> Self {
Self { Self {
notify: PubSubChannel::new(), notify: PubSubChannel::new(),
predict: Channel::new(), predict: PubSubChannel::new(),
telemetry: PubSubChannel::new()
} }
} }
} }

10
src/graphics/oled_ui.rs
View File

@@ -18,7 +18,7 @@ use nalgebra::Vector2;
use micromath::F32Ext; use micromath::F32Ext;
use embedded_graphics::geometry::OriginDimensions; use embedded_graphics::geometry::OriginDimensions;
use crate::events::{SensorSource, SensorState}; use crate::events::{Personality, SensorSource, SensorState};
use crate::graphics::images; use crate::graphics::images;
use crate::{ego::engine::MotionState, events::Scene}; use crate::{ego::engine::MotionState, events::Scene};
@@ -26,12 +26,10 @@ use crate::{ego::engine::MotionState, events::Scene};
pub struct OledUI { pub struct OledUI {
pub scene: Scene, pub scene: Scene,
pub motion: MotionState, pub motion: MotionState,
pub brakelight: bool, pub personality: Personality,
pub headlight: bool,
pub sensor_states: EnumMap<SensorSource, SensorState>, pub sensor_states: EnumMap<SensorSource, SensorState>,
pub velocity: f32, pub velocity: f32,
pub location: Vector2<f64>, pub location: Vector2<f64>,
pub sleep: bool,
} }
#[derive(Default, Debug)] #[derive(Default, Debug)]
@@ -179,12 +177,12 @@ impl Screen {
// The main UI content // The main UI content
Image::new(&images::BIKE, Point::new((128 / 2 - images::BIKE.size().width / 2) as i32, 24)).draw(sampler).unwrap(); Image::new(&images::BIKE, Point::new((128 / 2 - images::BIKE.size().width / 2) as i32, 24)).draw(sampler).unwrap();
let headlight_img = if state.ui.headlight { let headlight_img = if state.ui.personality == Personality::Active {
&images::HEADLIGHT_ON &images::HEADLIGHT_ON
} else { } else {
&images::HEADLIGHT_OFF &images::HEADLIGHT_OFF
}; };
let brakelight_img = if state.ui.brakelight { let brakelight_img = if state.ui.personality == Personality::Active {
&images::BRAKELIGHT_ON &images::BRAKELIGHT_ON
} else { } else {
&images::BRAKELIGHT_OFF &images::BRAKELIGHT_OFF

1
src/tasks/mod.rs
View File

@@ -12,7 +12,6 @@ pub mod demo;
pub mod oled_render; pub mod oled_render;
// Prediction engines // Prediction engines
pub mod predict;
pub mod motion; pub mod motion;
// Graphics stack // Graphics stack

8
src/tasks/motion.rs
View File

@@ -1,10 +1,10 @@
use embassy_sync::{channel::{DynamicReceiver, DynamicSender}, pubsub::DynPublisher}; use embassy_sync::{channel::DynamicReceiver, pubsub::DynPublisher};
use log::*; use log::*;
use crate::{ego::engine::BikeStates, events::{Measurement, Notification, Prediction, SensorSource, SensorState}}; use crate::{ego::engine::BikeStates, events::{Measurement, Prediction}};
#[embassy_executor::task] #[embassy_executor::task]
pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_sink: DynamicSender<'static, Prediction>) { pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_sink: DynPublisher<'static, Prediction>) {
let mut states = BikeStates::default(); let mut states = BikeStates::default();
loop { loop {
@@ -26,7 +26,7 @@ pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_
// FIXME: This needs harmonized with the automatic data timeout from above, somehow? // FIXME: This needs harmonized with the automatic data timeout from above, somehow?
Measurement::SensorHardwareStatus(source, state) => { Measurement::SensorHardwareStatus(source, state) => {
warn!("Sensor {source:?} reports {state:?}!"); warn!("Sensor {source:?} reports {state:?}!");
prediction_sink.send(Prediction::SensorStatus(source, state)).await; prediction_sink.publish(Prediction::SensorStatus(source, state)).await;
}, },
Measurement::SimulationProgress(source, duration, _pct) => debug!("{source:?} simulation time: {}", duration.as_secs()), Measurement::SimulationProgress(source, duration, _pct) => debug!("{source:?} simulation time: {}", duration.as_secs()),
Measurement::Annotation => () Measurement::Annotation => ()

54
src/tasks/oled.rs
View File

@@ -1,13 +1,12 @@
use alloc::sync::Arc; use alloc::sync::Arc;
use display_interface::DisplayError;
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex, pubsub::DynSubscriber}; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex, pubsub::DynSubscriber};
use embassy_time::{Duration, Instant, Timer}; use embassy_time::{Duration, Timer};
use embedded_graphics::{pixelcolor::BinaryColor, prelude::DrawTarget}; use embedded_graphics::pixelcolor::BinaryColor;
use figments::{mappings::embedded_graphics::Matrix2DSpace, prelude::{Coordinates, Rectangle}, render::{RenderSource, Shader}, surface::{BufferedSurfacePool, NullSurface, NullBufferPool, Surface, SurfaceBuilder, Surfaces}}; use figments::{mappings::embedded_graphics::Matrix2DSpace, prelude::{Coordinates, Rectangle}, render::Shader, surface::{BufferedSurfacePool, NullBufferPool, Surface, SurfaceBuilder, Surfaces}};
use figments_render::output::{Brightness, OutputAsync}; use figments_render::output::Brightness;
use log::*; use log::*;
use crate::{animation::Animation, backoff::Backoff, events::{Notification, Prediction, SensorSource, SensorState, Telemetry}, graphics::{display::DisplayControls, oled_ui::{OledUniforms, Screen}}}; use crate::{animation::Animation, events::{Personality, Prediction}, graphics::{display::DisplayControls, oled_ui::{OledUniforms, Screen}}};
#[cfg(feature="oled")] #[cfg(feature="oled")]
pub type OledUiSurfacePool = BufferedSurfacePool<OledUniforms, Matrix2DSpace, BinaryColor>; pub type OledUiSurfacePool = BufferedSurfacePool<OledUniforms, Matrix2DSpace, BinaryColor>;
@@ -22,7 +21,7 @@ pub type LockedUniforms = Arc<Mutex<CriticalSectionRawMutex, OledUniforms>>;
pub struct OledUI<S: Surface + core::fmt::Debug> { pub struct OledUI<S: Surface + core::fmt::Debug> {
overlay: S, overlay: S,
controls: DisplayControls, controls: DisplayControls,
uniforms: Arc<Mutex<CriticalSectionRawMutex, OledUniforms>> uniforms: LockedUniforms
} }
struct OverlayShader {} struct OverlayShader {}
@@ -57,36 +56,33 @@ impl<S: core::fmt::Debug + Surface<CoordinateSpace = Matrix2DSpace, Pixel = Bina
FADE_OUT.apply(&mut self.overlay).await; FADE_OUT.apply(&mut self.overlay).await;
} }
pub async fn on_event(&mut self, event: Telemetry) { pub async fn on_event(&mut self, event: Prediction) {
match event { match event {
// Waking and sleeping Prediction::SetPersonality(personality) => {
Telemetry::Notification(Notification::Sleep) => { match personality {
warn!("Putting OLED display to sleep"); Personality::Waking => {
self.screen_transition(Screen::Sleeping).await;
Timer::after_secs(1).await;
self.screen_transition(Screen::Blank).await;
self.controls.set_on(false);
//ui_state.sleep = true
},
Telemetry::Notification(Notification::WakeUp) => {
warn!("Waking up OLED display"); warn!("Waking up OLED display");
self.controls.set_on(true); self.controls.set_on(true);
self.screen_transition(Screen::Waking).await; self.screen_transition(Screen::Waking).await;
Timer::after_secs(1).await; Timer::after_secs(1).await;
self.screen_transition(Screen::Home).await; self.screen_transition(Screen::Home).await;
//ui_state.sleep = false
}, },
Personality::Sleeping => {
// State updates warn!("Putting OLED display to sleep");
Telemetry::Prediction(Prediction::Velocity(v)) => self.with_uniforms(|state| {state.ui.velocity = v;}).await, self.screen_transition(Screen::Sleeping).await;
Telemetry::Prediction(Prediction::Location(loc)) => self.with_uniforms(|state| {state.ui.location = loc}).await, Timer::after_secs(1).await;
Telemetry::Prediction(Prediction::Motion(motion)) => self.with_uniforms(|state| {state.ui.motion = motion}).await, self.screen_transition(Screen::Blank).await;
Telemetry::Notification(Notification::SceneChange(scene)) => self.with_uniforms(|state| {state.ui.scene = scene}).await, self.controls.set_on(false);
Telemetry::Notification(Notification::SetBrakelight(b)) => self.with_uniforms(|state| {state.ui.brakelight = b}).await, },
Telemetry::Notification(Notification::SetHeadlight(b)) => self.with_uniforms(|state| {state.ui.headlight = b}).await,
Telemetry::Notification(Notification::SensorStatus(src, sensor_state)) => self.with_uniforms(|state| {state.ui.sensor_states[src] = sensor_state}).await,
_ => () _ => ()
} }
self.with_uniforms(|state| { state.ui.personality = personality }).await;
},
Prediction::Velocity(v) => self.with_uniforms(|state| {state.ui.velocity = v;}).await,
Prediction::Location(loc) => self.with_uniforms(|state| {state.ui.location = loc}).await,
Prediction::Motion{ prev: _, next: motion } => self.with_uniforms(|state| {state.ui.motion = motion}).await,
Prediction::SensorStatus(src, sensor_state, ) => self.with_uniforms(|state| {state.ui.sensor_states[src] = sensor_state}).await,
}
} }
pub async fn with_uniforms(&self, f: impl Fn(&mut OledUniforms)) { pub async fn with_uniforms(&self, f: impl Fn(&mut OledUniforms)) {
@@ -97,7 +93,7 @@ impl<S: core::fmt::Debug + Surface<CoordinateSpace = Matrix2DSpace, Pixel = Bina
} }
#[embassy_executor::task] #[embassy_executor::task]
pub async fn oled_ui(mut events: DynSubscriber<'static, Telemetry>, mut ui: OledUI<OledSurface>) { pub async fn oled_ui(mut events: DynSubscriber<'static, Prediction>, mut ui: OledUI<OledSurface>) {
ui.screen_transition(Screen::Bootsplash).await; ui.screen_transition(Screen::Bootsplash).await;
Timer::after_secs(3).await; Timer::after_secs(3).await;

112
src/tasks/predict.rs
View File

@@ -1,112 +0,0 @@
use embassy_sync::{channel::DynamicReceiver, pubsub::DynPublisher};
use embassy_time::Duration;
use log::*;
use crate::{ego::engine::{gps_to_local_meters_haversine, MotionState}, events::{Notification, Prediction, Scene, Telemetry}, idle::IdleClock};
#[embassy_executor::task]
pub async fn prediction_task(prediction_src: DynamicReceiver<'static, Prediction>, notify: DynPublisher<'static, Notification>, telemetery: DynPublisher<'static, Telemetry>) {
let mut last_velocity = 0.0;
let mut first_position = None;
let mut last_position = Default::default();
let mut parking_timer = IdleClock::new(Duration::from_secs(10));
let mut sleep_timer = IdleClock::new(Duration::from_secs(30));
let mut stationary = true;
loop {
let d = first_position.map(|x| {
gps_to_local_meters_haversine(&x, &last_position).norm()
});
if let Ok(next_evt) = embassy_time::with_timeout(Duration::from_secs(1), prediction_src.receive()).await {
telemetery.publish(Telemetry::Prediction(next_evt)).await;
match next_evt {
Prediction::WakeRequested => {
if sleep_timer.wake() {
warn!("Wake requested during sleep");
notify.publish(Notification::WakeUp).await;
notify.publish(Notification::SetHeadlight(true)).await;
notify.publish(Notification::SetBrakelight(true)).await;
// Also reset the parking timer
parking_timer.wake();
} else if parking_timer.wake() {
info!("Wake requested while parked");
// If we weren't asleep but we were parked, then switch back to the Ready state and turn on the lights
notify.publish(Notification::SetHeadlight(true)).await;
notify.publish(Notification::SetBrakelight(true)).await;
notify.publish(Notification::SceneChange(Scene::Ready)).await;
}
}
Prediction::Velocity(v) => {
last_velocity = v;
// TODO: Probably makes sense to only print this based on an IdleTimer, so that a long period of slightly variable movement doesn't get lost, but we can still report values to the UI / telemetry outputs
//info!("Velocity predict: velocity={v}\tpos={last_position:?}\tdistance={d:?}");
},
Prediction::Location(loc) => {
if first_position.is_none() {
info!("Got location={loc:?}");
first_position = Some(loc);
}
last_position = loc;
}
Prediction::Motion(motion) => {
info!("Motion predict:\t{motion:?}\tvelocity={last_velocity}\tpos={last_position:?}\tdistance={d:?}");
if sleep_timer.wake() {
notify.publish(Notification::WakeUp).await;
notify.publish(Notification::SetHeadlight(true)).await;
notify.publish(Notification::SetBrakelight(true)).await
}
if parking_timer.wake() {
notify.publish(Notification::SetHeadlight(true)).await;
notify.publish(Notification::SetBrakelight(true)).await
}
match motion {
MotionState::Accelerating => {
if stationary {
// If we are going from standing still to immediately accelerating, first transition to the 'ready' scene
notify.publish(Notification::SceneChange(Scene::Ready)).await;
}
notify.publish(Notification::SceneChange(Scene::Accelerating)).await;
stationary = false;
},
MotionState::Decelerating => {
if stationary {
// If we are going from standing still to immediately accelerating, first transition to the 'ready' scene
notify.publish(Notification::SceneChange(Scene::Ready)).await;
}
notify.publish(Notification::SceneChange(Scene::Decelerating)).await;
stationary = false;
},
MotionState::Steady => {
notify.publish(Notification::SceneChange(Scene::Ready)).await;
stationary = false;
},
MotionState::Stationary => {
notify.publish(Notification::SceneChange(Scene::Ready)).await;
stationary = true
}
}
},
Prediction::SensorStatus(src, status) => {
notify.publish(Notification::SensorStatus(src, status)).await;
}
}
}
// TODO: Need a way to detect if sensors are dead for some reason. Probably should be done in the motion engine, since it would be a prediction?
if stationary {
if parking_timer.check() {
warn!("Engaging parking brake");
notify.publish(Notification::SceneChange(Scene::Idle)).await;
notify.publish(Notification::SetHeadlight(false)).await;
notify.publish(Notification::SetBrakelight(false)).await
}
if sleep_timer.check() {
warn!("Sleeping!");
notify.publish(Notification::Sleep).await;
}
}
}
}

60
src/tasks/safetyui.rs
View File

@@ -1,5 +1,5 @@
use embassy_sync::pubsub::DynSubscriber; use embassy_sync::pubsub::DynSubscriber;
use embassy_time::{Duration, Timer}; use embassy_time::Duration;
use figments::prelude::*; use figments::prelude::*;
use figments_render::output::Brightness; use figments_render::output::Brightness;
use rgb::Rgba; use rgb::Rgba;
@@ -7,7 +7,7 @@ use core::fmt::Debug;
use futures::join; use futures::join;
use log::*; use log::*;
use crate::{animation::{AnimDisplay, AnimatedSurface, Animation}, events::{Notification, Prediction}, graphics::{display::{DisplayControls, SegmentSpace, Uniforms}, shaders::*}, tasks::ui::UiSurfacePool}; use crate::{animation::{AnimDisplay, AnimatedSurface, Animation}, events::{Personality, Prediction}, graphics::{display::{DisplayControls, SegmentSpace, Uniforms}, shaders::*}, tasks::ui::UiSurfacePool};
#[derive(Debug)] #[derive(Debug)]
pub struct SafetyUi<S: Surface> { pub struct SafetyUi<S: Surface> {
@@ -92,43 +92,35 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
info!("Fade out overlay"); info!("Fade out overlay");
TURN_OFF.apply(&mut self.overlay).await; TURN_OFF.apply(&mut self.overlay).await;
self.overlay.set_visible(false); self.overlay.set_visible(false);
Timer::after_secs(3).await;
warn!("Turning off safety lights");
join!(
TURN_OFF.apply(&mut self.headlight),
TURN_OFF.apply(&mut self.brakelight)
);
info!("Wakeup complete!"); info!("Wakeup complete!");
} }
pub async fn on_event(&mut self, event: Notification) { pub async fn on_event(&mut self, event: Prediction) {
match event { if let Prediction::SetPersonality(personality) = event { match personality {
Notification::SceneChange(_) => (), // We already log this inside apply_scene() Personality::Active => {
evt => info!("SafetyUI event: {evt:?}")
}
match event {
// Toggling head and brake lights
// FIXME: These should be a Off/Low/High enum, so the stopping brake looks different from the dayrunning brake. // FIXME: These should be a Off/Low/High enum, so the stopping brake looks different from the dayrunning brake.
Notification::SetBrakelight(is_on) => { warn!("Active personality: Turning on safety lights");
if is_on { join!(
TURN_ON.apply(&mut self.brakelight).await; TURN_ON.apply(&mut self.brakelight),
} else { TURN_ON.apply(&mut self.headlight)
TURN_OFF.apply(&mut self.brakelight).await; );
}
}, },
Notification::SetHeadlight(is_on) => { Personality::Parked => {
if is_on { warn!("Idle personality: Turning off safety lights");
TURN_ON.apply(&mut self.headlight).await; join!(
} else { TURN_OFF.apply(&mut self.brakelight),
TURN_OFF.apply(&mut self.headlight).await; TURN_OFF.apply(&mut self.headlight)
} );
}, },
Personality::Sleeping => {
Notification::Sleep => self.sleep().await, warn!("Sleeping personality: Safety UI is going to sleep");
Notification::WakeUp => self.wake().await, self.sleep().await;
_ => () },
} Personality::Waking => {
warn!("Waking personality: Waking up safety UI");
self.wake().await;
},
} }
} }
} }
@@ -136,7 +128,7 @@ const TURN_ON: Animation = Animation::new().duration(Duration::from_secs(1)).fro
const TURN_OFF: Animation = Animation::new().duration(Duration::from_secs(1)).from(255).to(0); const TURN_OFF: Animation = Animation::new().duration(Duration::from_secs(1)).from(255).to(0);
#[embassy_executor::task] #[embassy_executor::task]
pub async fn safety_ui_main(mut events: DynSubscriber<'static, Notification>, mut ui: SafetyUi<<UiSurfacePool as Surfaces<SegmentSpace>>::Surface>) { pub async fn safety_ui_main(mut events: DynSubscriber<'static, Prediction>, mut ui: SafetyUi<<UiSurfacePool as Surfaces<SegmentSpace>>::Surface>) {
// Wait for the renderer to start running // Wait for the renderer to start running
//ui.display.render_is_running.wait().await; //ui.display.render_is_running.wait().await;
trace!("spooling until render starts ui={ui:?}"); trace!("spooling until render starts ui={ui:?}");

38
src/tasks/ui.rs
View File

@@ -1,4 +1,4 @@
use embassy_sync::pubsub::{DynPublisher, DynSubscriber}; use embassy_sync::pubsub::DynSubscriber;
use embassy_time::{Duration, Timer}; use embassy_time::{Duration, Timer};
use figments::prelude::*; use figments::prelude::*;
use rgb::{Rgb, Rgba}; use rgb::{Rgb, Rgba};
@@ -6,7 +6,7 @@ use core::fmt::Debug;
use futures::join; use futures::join;
use log::*; use log::*;
use crate::{animation::{AnimatedSurface, Animation}, events::{Notification, Scene, SensorSource, SensorState, Telemetry}, graphics::{display::{SegmentSpace, Uniforms}, shaders::*}}; use crate::{animation::{AnimatedSurface, Animation}, ego::engine::MotionState, events::{Personality, Prediction, Scene, SensorSource, SensorState}, graphics::{display::{SegmentSpace, Uniforms}, shaders::*}};
pub struct Ui<S: Surface> { pub struct Ui<S: Surface> {
// Background layer provides an always-running background for everything to draw on // Background layer provides an always-running background for everything to draw on
@@ -132,23 +132,23 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
} }
} }
pub async fn on_event(&mut self, event: Notification) { pub async fn on_event(&mut self, event: Prediction) {
match event { match event {
Notification::SceneChange(_) => (), // We already log this inside apply_scene() Prediction::SetPersonality(personality) => match personality {
evt => info!("UI event: {evt:?}") Personality::Active => self.apply_scene(Scene::Ready).await,
} Personality::Parked => self.apply_scene(Scene::Idle).await,
match event { Personality::Waking => self.show().await,
// TODO: We probably also want some events to indicate when the ESP has no calibration data or otherwise needs re-calibrated and is waiting for the bike to stand still
Notification::SensorStatus(SensorSource::IMU, SensorState::Online) => self.flash_notification_color(Rgb::new(0, 255, 0)).await,
Notification::SensorStatus(SensorSource::Location, SensorState::Degraded) => self.flash_notification_color(Rgb::new(255, 0, 0)).await,
Notification::SensorStatus(SensorSource::GPS, SensorState::Online) => self.flash_notification_color(Rgb::new(0, 255, 255)).await,
// Scene change
Notification::SceneChange(scene) => self.apply_scene(scene).await,
Notification::WakeUp => self.show().await,
_ => () _ => ()
},
Prediction::Motion { prev: _, next: MotionState::Accelerating } => self.apply_scene(Scene::Accelerating).await,
Prediction::Motion { prev: _, next: MotionState::Decelerating } => self.apply_scene(Scene::Decelerating).await,
Prediction::Motion { prev: _, next: MotionState::Steady } => self.apply_scene(Scene::Ready).await,
Prediction::Motion { prev: _, next: MotionState::Stationary } => self.apply_scene(Scene::Ready).await,
Prediction::SensorStatus(SensorSource::IMU, SensorState::Online) => self.flash_notification_color(Rgb::new(0, 255, 0)).await,
Prediction::SensorStatus(SensorSource::Location, SensorState::Degraded) => self.flash_notification_color(Rgb::new(255, 0, 0)).await,
Prediction::SensorStatus(SensorSource::GPS, SensorState::Online) => self.flash_notification_color(Rgb::new(0, 255, 255)).await,
_ => ()
}
// Other event ideas: // Other event ideas:
// - Bike has crashed, or is laid down // - Bike has crashed, or is laid down
// - Unstable physics right before crashing? // - Unstable physics right before crashing?
@@ -160,7 +160,6 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
// - Bike is waking up from stationary? // - Bike is waking up from stationary?
} }
} }
}
impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pixel = Rgba<u8>>> Surface for Ui<S> { impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pixel = Rgba<u8>>> Surface for Ui<S> {
type Uniforms = S::Uniforms; type Uniforms = S::Uniforms;
@@ -200,7 +199,7 @@ pub type UiSurfacePool = NullBufferPool<Uniforms, SegmentSpace, Rgba<u8>>;
pub type UiSurfacePool = BufferedSurfacePool<Uniforms, SegmentSpace, Rgba<u8>>; pub type UiSurfacePool = BufferedSurfacePool<Uniforms, SegmentSpace, Rgba<u8>>;
#[embassy_executor::task] #[embassy_executor::task]
pub async fn ui_main(mut events: DynSubscriber<'static, Notification>, telemetery: DynPublisher<'static, Telemetry>, mut ui: Ui<<UiSurfacePool as Surfaces<SegmentSpace>>::Surface>) { pub async fn ui_main(mut events: DynSubscriber<'static, Prediction>, mut ui: Ui<<UiSurfacePool as Surfaces<SegmentSpace>>::Surface>) {
// FIXME: This should instead wait on some kind of flag set by the safety UI, or else we risk painting before we even have a display up and running // FIXME: This should instead wait on some kind of flag set by the safety UI, or else we risk painting before we even have a display up and running
Timer::after_secs(3).await; Timer::after_secs(3).await;
ui.show().await; ui.show().await;
@@ -212,6 +211,5 @@ pub async fn ui_main(mut events: DynSubscriber<'static, Notification>, telemeter
loop { loop {
let evt = events.next_message_pure().await; let evt = events.next_message_pure().await;
ui.on_event(evt).await; ui.on_event(evt).await;
telemetery.publish(Telemetry::Notification(evt)).await;
} }
} }

6
src/tasks/wifi.rs
View File

@@ -13,7 +13,7 @@ use nalgebra::Vector2;
use reqwless::client::{HttpClient, TlsConfig}; use reqwless::client::{HttpClient, TlsConfig};
use static_cell::StaticCell; use static_cell::StaticCell;
use crate::{backoff::Backoff, events::{Prediction, Telemetry}}; use crate::{backoff::Backoff, events::{Prediction}};
#[embassy_executor::task] #[embassy_executor::task]
async fn net_task(mut runner: embassy_net::Runner<'static, WifiDevice<'static>>) { async fn net_task(mut runner: embassy_net::Runner<'static, WifiDevice<'static>>) {
@@ -25,7 +25,7 @@ static RESOURCES: StaticCell<StackResources<5>> = StaticCell::new();
// TODO: Wifi task needs to know when there is data to upload, so it only connects when needed. // TODO: Wifi task needs to know when there is data to upload, so it only connects when needed.
#[embassy_executor::task] #[embassy_executor::task]
pub async fn wireless_task(mut telemetry: DynSubscriber<'static, Telemetry>, wifi_init: &'static mut Controller<'static>, wifi_device: esp_hal::peripherals::WIFI<'static>) { pub async fn wireless_task(mut predictions: DynSubscriber<'static, Prediction>, wifi_init: &'static mut Controller<'static>, wifi_device: esp_hal::peripherals::WIFI<'static>) {
let (mut wifi, interfaces) = esp_radio::wifi::new(wifi_init, wifi_device, esp_radio::wifi::Config::default()) let (mut wifi, interfaces) = esp_radio::wifi::new(wifi_init, wifi_device, esp_radio::wifi::Config::default())
.expect("Failed to initialize WIFI!"); .expect("Failed to initialize WIFI!");
wifi.set_config(&esp_radio::wifi::ModeConfig::Client( wifi.set_config(&esp_radio::wifi::ModeConfig::Client(
@@ -81,7 +81,7 @@ pub async fn wireless_task(mut telemetry: DynSubscriber<'static, Telemetry>, wif
let mut client = HttpClient::new_with_tls(&tcp, &dns, tls); let mut client = HttpClient::new_with_tls(&tcp, &dns, tls);
loop { loop {
if let Telemetry::Prediction(Prediction::Location(coords)) = telemetry.next_message_pure().await { if let Prediction::Location(coords) = predictions.next_message_pure().await {
if let Err(e) = push_location(&mut client, coords).await { if let Err(e) = push_location(&mut client, coords).await {
error!("HTTP error in publishing location: {e:?}"); error!("HTTP error in publishing location: {e:?}");
break break