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() });
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");
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");
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");
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")]
{
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")]
@@ -179,11 +179,6 @@ async fn main(spawner: Spawner) {
warn!("Launching with demo sequencer");
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");
spawner.must_spawn(wdt_task(ui_wdt));

105
src/ego/engine.rs
View File

@@ -5,7 +5,7 @@ use log::*;
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)]
pub enum MotionState {
@@ -37,8 +37,10 @@ pub struct BikeStates {
predicted_velocity: Breaker<f32>,
reported_velocity: Breaker<f32>,
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 {
@@ -50,7 +52,9 @@ impl Debug for BikeStates {
.field("motion_state", &self.motion_state)
.field("predicted_location", &self.predicted_location)
.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()
}
}
@@ -95,7 +99,7 @@ impl BikeStates {
let heading_rotation = Rotation3::from_axis_angle(&Vector3::z_axis(), self.heading.heading());
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);
} else {
// 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() {
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;
predictions.publish(Prediction::SensorStatus(SensorSource::ForwardsReference, SensorState::AcquiringFix)).await;
predictions.publish(Prediction::SensorStatus(SensorSource::GravityReference, SensorState::AcquiringFix)).await;
predictions.publish(Prediction::SensorStatus(SensorSource::Location, SensorState::AcquiringFix)).await;
}
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() {
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() {
None => (),
Some(true) => predictions.send(Prediction::SensorStatus(SensorSource::Location, SensorState::Online)).await,
Some(false) => predictions.send(Prediction::SensorStatus(SensorSource::Location, SensorState::Degraded)).await,
Some(true) => predictions.publish(Prediction::SensorStatus(SensorSource::Location, SensorState::Online)).await,
Some(false) => predictions.publish(Prediction::SensorStatus(SensorSource::Location, SensorState::Degraded)).await,
}
let est = self.kf.x;
@@ -140,29 +146,43 @@ impl BikeStates {
if let Some(pos) = position {
self.predicted_location.set(pos);
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());
if let Some(v) = self.predicted_velocity.read_tripped() {
self.wake_requested.set(true);
if self.wake_requested.read_tripped().is_some() {
predictions.send(Prediction::WakeRequested).await;
}
self.speedo.insert(v);
if let Some(current_prediction) = self.predicted_velocity.read_tripped() {
// Add the prediction into the speedometer model
self.speedo.insert(current_prediction);
// If the model has enough samples to report useful data, we can start analyzing the motion trends
if self.speedo.is_filled() {
let threshold = 1.0;
// Calculate if the velocity is increasing, decreasing, or mostly the same
let trend = self.speedo.data().windows(2).map(|n| {
n[1] - n[0]
}).sum::<f32>();
// Also grab the average velocity of the last few sample periods
let mean = self.speedo.mean();
// Reported velocity is kept only to the first decimal
self.reported_velocity.set((mean *10.0).round() / 10.0);
if let Some(v) = self.reported_velocity.read_tripped() {
predictions.send(Prediction::Velocity(v)).await;
// 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).trunc() / 10.0);
if let Some(reported) = self.reported_velocity.read_tripped() {
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.
@@ -175,19 +195,31 @@ impl BikeStates {
} 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
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!
self.motion_state.set(MotionState::Stationary);
}
// And if the motion status has changed, send it out
if let Some(state) = self.motion_state.read_tripped() {
debug!("state={state:?} trend={trend} mean={mean} v={v}");
predictions.send(Prediction::Motion(state)).await;
}
}
} else {
self.wake_requested.set(false);
}
// And if the motion status has changed, send it out
if let Some(state) = self.motion_state.read_tripped() {
//debug!("state={state:?} trend={trend} mean={mean} v={v}");
//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;
}
}
}
}
@@ -210,8 +242,9 @@ impl Default for BikeStates {
predicted_location: Default::default(),
predicted_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 enum_map::Enum;
use enumset::EnumSetType;
use nalgebra::{Vector2, Vector3};
use crate::{graphics::display::DisplayControls, ego::engine::MotionState};
use crate::ego::engine::MotionState;
#[derive(Clone, Copy, Default, Debug)]
pub enum Scene {
@@ -47,14 +47,24 @@ pub enum Measurement {
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)]
pub enum Prediction {
Motion(MotionState),
Motion { prev: MotionState, next: MotionState },
Velocity(f32),
Location(Vector2<f64>),
WakeRequested,
// States of external connections to the world
SensorStatus(SensorSource, SensorState),
// The system should enter into one of the four personalities
SetPersonality(Personality)
}
#[derive(Clone, Copy, Debug)]
@@ -77,12 +87,6 @@ pub enum Notification {
Beat,
}
#[derive(Clone, Copy, Debug)]
pub enum Telemetry {
Notification(Notification),
Prediction(Prediction),
}
// GPS data = 2, motion data = 1
#[derive(Debug, EnumSetType, Enum)]
pub enum SensorSource {
@@ -117,16 +121,14 @@ impl TryFrom<i8> for SensorSource {
#[derive(Debug)]
pub struct BusGarage {
pub notify: PubSubChannel<NoopRawMutex, Notification, 5, 2, 4>,
pub predict: Channel<NoopRawMutex, Prediction, 15>,
pub telemetry: PubSubChannel<NoopRawMutex, Telemetry, 15, 2, 4>
pub predict: PubSubChannel<NoopRawMutex, Prediction, 15, 3, 3>,
}
impl Default for BusGarage {
fn default() -> Self {
Self {
notify: PubSubChannel::new(),
predict: Channel::new(),
telemetry: PubSubChannel::new()
predict: PubSubChannel::new(),
}
}
}

10
src/graphics/oled_ui.rs
View File

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

1
src/tasks/mod.rs
View File

@@ -12,7 +12,6 @@ pub mod demo;
pub mod oled_render;
// Prediction engines
pub mod predict;
pub mod motion;
// 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 crate::{ego::engine::BikeStates, events::{Measurement, Notification, Prediction, SensorSource, SensorState}};
use crate::{ego::engine::BikeStates, events::{Measurement, Prediction}};
#[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();
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?
Measurement::SensorHardwareStatus(source, 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::Annotation => ()

66
src/tasks/oled.rs
View File

@@ -1,13 +1,12 @@
use alloc::sync::Arc;
use display_interface::DisplayError;
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex, pubsub::DynSubscriber};
use embassy_time::{Duration, Instant, Timer};
use embedded_graphics::{pixelcolor::BinaryColor, prelude::DrawTarget};
use figments::{mappings::embedded_graphics::Matrix2DSpace, prelude::{Coordinates, Rectangle}, render::{RenderSource, Shader}, surface::{BufferedSurfacePool, NullSurface, NullBufferPool, Surface, SurfaceBuilder, Surfaces}};
use figments_render::output::{Brightness, OutputAsync};
use embassy_time::{Duration, Timer};
use embedded_graphics::pixelcolor::BinaryColor;
use figments::{mappings::embedded_graphics::Matrix2DSpace, prelude::{Coordinates, Rectangle}, render::Shader, surface::{BufferedSurfacePool, NullBufferPool, Surface, SurfaceBuilder, Surfaces}};
use figments_render::output::Brightness;
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")]
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> {
overlay: S,
controls: DisplayControls,
uniforms: Arc<Mutex<CriticalSectionRawMutex, OledUniforms>>
uniforms: LockedUniforms
}
struct OverlayShader {}
@@ -57,35 +56,32 @@ impl<S: core::fmt::Debug + Surface<CoordinateSpace = Matrix2DSpace, Pixel = Bina
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 {
// Waking and sleeping
Telemetry::Notification(Notification::Sleep) => {
warn!("Putting OLED display to sleep");
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
Prediction::SetPersonality(personality) => {
match personality {
Personality::Waking => {
warn!("Waking up OLED display");
self.controls.set_on(true);
self.screen_transition(Screen::Waking).await;
Timer::after_secs(1).await;
self.screen_transition(Screen::Home).await;
},
Personality::Sleeping => {
warn!("Putting OLED display to sleep");
self.screen_transition(Screen::Sleeping).await;
Timer::after_secs(1).await;
self.screen_transition(Screen::Blank).await;
self.controls.set_on(false);
},
_ => ()
}
self.with_uniforms(|state| { state.ui.personality = personality }).await;
},
Telemetry::Notification(Notification::WakeUp) => {
warn!("Waking up OLED display");
self.controls.set_on(true);
self.screen_transition(Screen::Waking).await;
Timer::after_secs(1).await;
self.screen_transition(Screen::Home).await;
//ui_state.sleep = false
},
// State updates
Telemetry::Prediction(Prediction::Velocity(v)) => self.with_uniforms(|state| {state.ui.velocity = v;}).await,
Telemetry::Prediction(Prediction::Location(loc)) => self.with_uniforms(|state| {state.ui.location = loc}).await,
Telemetry::Prediction(Prediction::Motion(motion)) => self.with_uniforms(|state| {state.ui.motion = motion}).await,
Telemetry::Notification(Notification::SceneChange(scene)) => self.with_uniforms(|state| {state.ui.scene = scene}).await,
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,
_ => ()
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,
}
}
@@ -97,7 +93,7 @@ impl<S: core::fmt::Debug + Surface<CoordinateSpace = Matrix2DSpace, Pixel = Bina
}
#[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;
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;
}
}
}
}

62
src/tasks/safetyui.rs
View File

@@ -1,5 +1,5 @@
use embassy_sync::pubsub::DynSubscriber;
use embassy_time::{Duration, Timer};
use embassy_time::Duration;
use figments::prelude::*;
use figments_render::output::Brightness;
use rgb::Rgba;
@@ -7,7 +7,7 @@ use core::fmt::Debug;
use futures::join;
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)]
pub struct SafetyUi<S: Surface> {
@@ -92,43 +92,35 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
info!("Fade out overlay");
TURN_OFF.apply(&mut self.overlay).await;
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!");
}
pub async fn on_event(&mut self, event: Notification) {
match event {
Notification::SceneChange(_) => (), // We already log this inside apply_scene()
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.
Notification::SetBrakelight(is_on) => {
if is_on {
TURN_ON.apply(&mut self.brakelight).await;
} else {
TURN_OFF.apply(&mut self.brakelight).await;
}
pub async fn on_event(&mut self, event: Prediction) {
if let Prediction::SetPersonality(personality) = event { match personality {
Personality::Active => {
// FIXME: These should be a Off/Low/High enum, so the stopping brake looks different from the dayrunning brake.
warn!("Active personality: Turning on safety lights");
join!(
TURN_ON.apply(&mut self.brakelight),
TURN_ON.apply(&mut self.headlight)
);
},
Notification::SetHeadlight(is_on) => {
if is_on {
TURN_ON.apply(&mut self.headlight).await;
} else {
TURN_OFF.apply(&mut self.headlight).await;
}
Personality::Parked => {
warn!("Idle personality: Turning off safety lights");
join!(
TURN_OFF.apply(&mut self.brakelight),
TURN_OFF.apply(&mut self.headlight)
);
},
Notification::Sleep => self.sleep().await,
Notification::WakeUp => self.wake().await,
_ => ()
}
Personality::Sleeping => {
warn!("Sleeping personality: Safety UI is going to sleep");
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);
#[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
//ui.display.render_is_running.wait().await;
trace!("spooling until render starts ui={ui:?}");

54
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 figments::prelude::*;
use rgb::{Rgb, Rgba};
@@ -6,7 +6,7 @@ use core::fmt::Debug;
use futures::join;
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> {
// Background layer provides an always-running background for everything to draw on
@@ -132,33 +132,32 @@ 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 {
Notification::SceneChange(_) => (), // We already log this inside apply_scene()
evt => info!("UI event: {evt:?}")
}
match event {
// 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::SetPersonality(personality) => match personality {
Personality::Active => self.apply_scene(Scene::Ready).await,
Personality::Parked => self.apply_scene(Scene::Idle).await,
Personality::Waking => 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:
// - Bike has crashed, or is laid down
// - Unstable physics right before crashing?
// - Turning left/right
// - BPM sync with phone app
// - GPS data is being synchronized with nextcloud/whatever
// - A periodic flash when re-initializing MPU and GPS, to indicate there might be a problem?
// - Bluetooth/BLE connect/disconnect events
// - Bike is waking up from stationary?
}
// Other event ideas:
// - Bike has crashed, or is laid down
// - Unstable physics right before crashing?
// - Turning left/right
// - BPM sync with phone app
// - GPS data is being synchronized with nextcloud/whatever
// - A periodic flash when re-initializing MPU and GPS, to indicate there might be a problem?
// - Bluetooth/BLE connect/disconnect events
// - Bike is waking up from stationary?
}
}
@@ -200,7 +199,7 @@ pub type UiSurfacePool = NullBufferPool<Uniforms, SegmentSpace, Rgba<u8>>;
pub type UiSurfacePool = BufferedSurfacePool<Uniforms, SegmentSpace, Rgba<u8>>;
#[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
Timer::after_secs(3).await;
ui.show().await;
@@ -212,6 +211,5 @@ pub async fn ui_main(mut events: DynSubscriber<'static, Notification>, telemeter
loop {
let evt = events.next_message_pure().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 static_cell::StaticCell;
use crate::{backoff::Backoff, events::{Prediction, Telemetry}};
use crate::{backoff::Backoff, events::{Prediction}};
#[embassy_executor::task]
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.
#[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())
.expect("Failed to initialize WIFI!");
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);
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 {
error!("HTTP error in publishing location: {e:?}");
break