diff --git a/src/bin/main.rs b/src/bin/main.rs index 73c9f1f..451c4f3 100644 --- a/src/bin/main.rs +++ b/src/bin/main.rs @@ -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)); diff --git a/src/ego/engine.rs b/src/ego/engine.rs index 94f4624..e6c2c3f 100644 --- a/src/ego/engine.rs +++ b/src/ego/engine.rs @@ -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, reported_velocity: Breaker, predicted_location: Breaker>, - wake_requested: Breaker, - 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::(); + // 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)) } } } diff --git a/src/events.rs b/src/events.rs index ffde1f1..8370d77 100644 --- a/src/events.rs +++ b/src/events.rs @@ -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), - 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 for SensorSource { #[derive(Debug)] pub struct BusGarage { pub notify: PubSubChannel, - pub predict: Channel, - pub telemetry: PubSubChannel + pub predict: PubSubChannel, } impl Default for BusGarage { fn default() -> Self { Self { notify: PubSubChannel::new(), - predict: Channel::new(), - telemetry: PubSubChannel::new() + predict: PubSubChannel::new(), } } } \ No newline at end of file diff --git a/src/graphics/oled_ui.rs b/src/graphics/oled_ui.rs index 70de972..a747690 100644 --- a/src/graphics/oled_ui.rs +++ b/src/graphics/oled_ui.rs @@ -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, pub velocity: f32, pub location: Vector2, - 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 diff --git a/src/tasks/mod.rs b/src/tasks/mod.rs index a9af01d..86e7a69 100644 --- a/src/tasks/mod.rs +++ b/src/tasks/mod.rs @@ -12,7 +12,6 @@ pub mod demo; pub mod oled_render; // Prediction engines -pub mod predict; pub mod motion; // Graphics stack diff --git a/src/tasks/motion.rs b/src/tasks/motion.rs index e5e7021..2b527f3 100644 --- a/src/tasks/motion.rs +++ b/src/tasks/motion.rs @@ -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 => () diff --git a/src/tasks/oled.rs b/src/tasks/oled.rs index e7ec4e3..cd97987 100644 --- a/src/tasks/oled.rs +++ b/src/tasks/oled.rs @@ -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; @@ -22,7 +21,7 @@ pub type LockedUniforms = Arc>; pub struct OledUI { overlay: S, controls: DisplayControls, - uniforms: Arc> + uniforms: LockedUniforms } struct OverlayShader {} @@ -57,35 +56,32 @@ impl { - 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, mut ui: OledUI) { +pub async fn oled_ui(mut events: DynSubscriber<'static, Prediction>, mut ui: OledUI) { ui.screen_transition(Screen::Bootsplash).await; Timer::after_secs(3).await; diff --git a/src/tasks/predict.rs b/src/tasks/predict.rs deleted file mode 100644 index 53a1b80..0000000 --- a/src/tasks/predict.rs +++ /dev/null @@ -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; - } - } - } -} \ No newline at end of file diff --git a/src/tasks/safetyui.rs b/src/tasks/safetyui.rs index 6b603df..c6e83d1 100644 --- a/src/tasks/safetyui.rs +++ b/src/tasks/safetyui.rs @@ -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 { @@ -92,43 +92,35 @@ impl (), // 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<>::Surface>) { +pub async fn safety_ui_main(mut events: DynSubscriber<'static, Prediction>, mut ui: SafetyUi<>::Surface>) { // Wait for the renderer to start running //ui.display.render_is_running.wait().await; trace!("spooling until render starts ui={ui:?}"); diff --git a/src/tasks/ui.rs b/src/tasks/ui.rs index d588f6e..b1f6926 100644 --- a/src/tasks/ui.rs +++ b/src/tasks/ui.rs @@ -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 { // Background layer provides an always-running background for everything to draw on @@ -132,33 +132,32 @@ impl (), // 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>; pub type UiSurfacePool = BufferedSurfacePool>; #[embassy_executor::task] -pub async fn ui_main(mut events: DynSubscriber<'static, Notification>, telemetery: DynPublisher<'static, Telemetry>, mut ui: Ui<>::Surface>) { +pub async fn ui_main(mut events: DynSubscriber<'static, Prediction>, mut ui: Ui<>::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; } } \ No newline at end of file diff --git a/src/tasks/wifi.rs b/src/tasks/wifi.rs index 52cbd86..02fb9bb 100644 --- a/src/tasks/wifi.rs +++ b/src/tasks/wifi.rs @@ -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> = 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