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

events: rewrite how sensor statuses are reported, and implement some oled UI icons for it

Browse Source
This commit is contained in:
Victoria Fischer
2025-11-08 12:04:22 +01:00
parent a36fe3d1ac
commit 092885f163
24 changed files with 845 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
src/ego/engine.rs
View File

@@ -5,7 +5,7 @@ use log::*;
use core::fmt::Debug;
use crate::{ego::{heading::HeadingEstimator, kalman::Ekf2D, orientation::OrientationEstimator}, events::{Notification, Prediction}, idle::IdleClock, Breaker, CircularBuffer};
use crate::{Breaker, CircularBuffer, ego::{heading::HeadingEstimator, kalman::Ekf2D, orientation::OrientationEstimator}, events::{Notification, Prediction, SensorSource, SensorState}, idle::IdleClock};
#[derive(PartialEq, Debug, Default, Clone, Copy)]
pub enum MotionState {
@@ -28,8 +28,10 @@ pub struct BikeStates {
reference_fix: Option<Vector2<f64>>, // The first GPS value, which is used to make sense out of the EKF output which is in meters offset from this point
// State switches
is_calibrated: Breaker<bool>,
has_down: Breaker<bool>,
has_forwards: Breaker<bool>,
motion_state: Breaker<MotionState>,
acquiring_data: Breaker<bool>,
// FIXME: pub
pub has_gps_fix: Breaker<bool>,
predicted_velocity: Breaker<f32>,
@@ -55,6 +57,7 @@ impl Debug for BikeStates {
impl BikeStates {
pub fn insert_gps(&mut self, gps_pos: Vector2<f64>) {
self.acquiring_data.set(true);
match self.reference_fix {
None => {
self.reference_fix = Some(gps_pos);
@@ -77,6 +80,7 @@ impl BikeStates {
}
pub fn insert_imu(&mut self, accel: Vector3<f32>, gyro: Vector3<f32>) {
self.acquiring_data.set(true);
self.orientation.insert(accel);
if self.orientation.has_down() {
@@ -98,19 +102,33 @@ impl BikeStates {
self.kf.update_zupt();
}
self.is_calibrated.set(true);
self.has_down.set(true);
if self.orientation.is_ready() {
self.has_forwards.set(true);
}
}
}
pub async fn commit(&mut self, predictions: &DynamicSender<'static, Prediction>, notifications: &DynPublisher<'static, Notification>) {
if let Some(true) = self.is_calibrated.read_tripped() {
notifications.publish(Notification::SensorOnline(crate::events::SensorSource::IMU)).await
if let Some(true) = self.acquiring_data.read_tripped() {
notifications.publish(Notification::SensorStatus(SensorSource::ForwardsReference, SensorState::AcquiringFix)).await;
notifications.publish(Notification::SensorStatus(SensorSource::GravityReference, SensorState::AcquiringFix)).await;
notifications.publish(Notification::SensorStatus(SensorSource::Location, SensorState::AcquiringFix)).await;
}
if let Some(true) = self.has_down.read_tripped() {
notifications.publish(Notification::SensorStatus(SensorSource::GravityReference, SensorState::Online)).await
}
if let Some(true) = self.has_forwards.read_tripped() {
notifications.publish(Notification::SensorStatus(SensorSource::ForwardsReference, SensorState::Online)).await
}
match self.has_gps_fix.read_tripped() {
None => (),
Some(true) => notifications.publish(Notification::SensorOnline(crate::events::SensorSource::GPS)).await,
Some(false) => notifications.publish(Notification::SensorOffline(crate::events::SensorSource::GPS)).await,
Some(true) => notifications.publish(Notification::SensorStatus(SensorSource::Location, SensorState::Online)).await,
Some(false) => notifications.publish(Notification::SensorStatus(SensorSource::Location, SensorState::Degraded)).await,
}
let est = self.kf.x;
@@ -182,7 +200,8 @@ impl Default for BikeStates {
last_stamp: Instant::now(),
speedo: Default::default(),
heading: Default::default(),
is_calibrated: Default::default(),
has_down: Default::default(),
has_forwards: Default::default(),
kf: Default::default(),
steady_timer: IdleClock::new(Duration::from_secs(3)),
last_fix: Default::default(),
@@ -191,7 +210,8 @@ impl Default for BikeStates {
predicted_location: Default::default(),
predicted_velocity: Default::default(),
reported_velocity: Default::default(),
wake_requested: Default::default()
wake_requested: Default::default(),
acquiring_data: Default::default()
}
}
}

42
src/events.rs
View File

@@ -1,6 +1,8 @@
use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::Channel, 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};
@@ -14,6 +16,22 @@ pub enum Scene {
Idle, // Default state when waking up from sleep, or entered when accelerometers and GPS both show zero motion for ~30 seconds
}
#[derive(Clone, Copy, Default, Debug)]
pub enum SensorState {
#[default]
// There is no connection to the sensor
Offline,
// Sensor is starting up
AcquiringFix,
// Sensor is ready and is generating data
Online,
// Sensor was previously fully functioning but currently is not (eg, gps fix lost)
Degraded,
}
#[derive(Clone, Copy, Debug)]
pub enum Measurement {
// GPS coordinates
@@ -22,8 +40,7 @@ pub enum Measurement {
IMU { accel: Vector3<f32>, gyro: Vector3<f32> },
// Hardware status updates
SensorOnline(SensorSource),
SensorOffline(SensorSource),
SensorHardwareStatus(SensorSource, SensorState),
// Simulation metadata updates
SimulationProgress(SensorSource, Duration, f32)
@@ -43,12 +60,7 @@ pub enum Notification {
SceneChange(Scene),
// States of external connections to the world
SensorOnline(SensorSource),
SensorOffline(SensorSource),
// TODO: Should be emitted by the prediction engine after it doesn't get any sensor data for some time. Perhaps the threads are somehow deadlocked and a reboot is needed if it doesn't recover
SensorsOffline,
SensorStatus(SensorSource, SensorState),
// The prediction engine has decided that the system should be woken up and begin running again
WakeUp,
// The prediction engine has decided that the system is inactive enough and it should go to low-power sleep
@@ -68,10 +80,20 @@ pub enum Telemetry {
Prediction(Prediction),
}
#[derive(Clone, Copy, Debug)]
#[derive(Debug, EnumSetType, Enum)]
pub enum SensorSource {
// Real hardware
IMU,
GPS
GPS,
// Fusion outputs
GravityReference,
ForwardsReference,
Location,
// Simulated sensors
Demo,
Simulation
}
#[derive(Debug)]

127
src/graphics/oled_ui.rs
View File

@@ -1,14 +1,15 @@
use core::f32::consts::PI;
use core::fmt::Binary;
use alloc::format;
use embedded_graphics::image::ImageRaw;
use embedded_graphics::mono_font::ascii::*;
use embedded_graphics::mono_font::{MonoTextStyle, MonoTextStyleBuilder};
use embedded_graphics::pixelcolor::BinaryColor;
use embedded_graphics::prelude::Size;
use embedded_graphics::prelude::{DrawTarget, Size};
use embedded_graphics::primitives::{Line, PrimitiveStyle, PrimitiveStyleBuilder, StyledDrawable};
use embedded_graphics::text::{Alignment, Text};
use embedded_graphics::{image::Image, prelude::Point, Drawable};
use enum_map::EnumMap;
use figments::liber8tion::trig::sin8;
use figments::mappings::embedded_graphics::Matrix2DSpace;
use figments::{liber8tion::trig::cos8, mappings::embedded_graphics::EmbeddedGraphicsSampler};
@@ -17,6 +18,7 @@ use nalgebra::Vector2;
use micromath::F32Ext;
use embedded_graphics::geometry::OriginDimensions;
use crate::events::{SensorSource, SensorState};
use crate::graphics::images;
use crate::{ego::engine::MotionState, events::Scene};
@@ -26,8 +28,7 @@ pub struct OledUI {
pub motion: MotionState,
pub brakelight: bool,
pub headlight: bool,
pub gps_online: bool,
pub imu_online: bool,
pub sensor_states: EnumMap<SensorSource, SensorState>,
pub velocity: f32,
pub location: Vector2<f64>,
pub sleep: bool,
@@ -50,6 +51,90 @@ pub enum Screen {
Waking
}
struct SensorImage {
source: SensorSource,
on: &'static ImageRaw<'static, BinaryColor>,
off: &'static ImageRaw<'static, BinaryColor>,
}
struct SensorIcon<'a> {
anchor: embedded_graphics::geometry::Point,
image: &'a SensorImage,
state: SensorState,
frame: usize
}
impl<'a> SensorIcon<'a> {
pub const fn new(image: &'a SensorImage, state: SensorState, frame: usize, anchor: embedded_graphics::geometry::Point) -> Self {
Self {
anchor,
image,
state,
frame
}
}
pub fn draw<T: DrawTarget<Color = BinaryColor>>(&self, target: &mut T) -> Result<(), T::Error> {
match self.state {
SensorState::AcquiringFix => {
if (self.frame / 10) % 2 == 0 {
Image::new(self.image.off, self.anchor).draw(target)
} else {
Ok(())
}
},
SensorState::Online => {
Image::new(self.image.on, self.anchor).draw(target)
},
SensorState::Offline => {
Image::new(self.image.off, self.anchor).draw(target)
},
SensorState::Degraded => {
if (self.frame / 10) % 2 == 0 {
Image::new(self.image.on, self.anchor).draw(target)
} else {
Ok(())
}
}
}
}
}
const SENSOR_IMAGES: &[SensorImage] = &[
SensorImage {
source: SensorSource::GPS,
on: &images::GPS_ON, off: &images::GPS_OFF,
},
SensorImage {
source: SensorSource::IMU,
on: &images::IMU_ON, off: &images::IMU_OFF,
},
SensorImage {
source: SensorSource::GravityReference,
on: &images::GRAVITY_LOCATED, off: &images::GRAVITY_MISSING,
},
SensorImage {
source: SensorSource::Location,
on: &images::LOCATION_ON, off: &images::LOCATION_OFF,
},
#[cfg(feature="demo")]
SensorImage {
source: SensorSource::Demo,
on: &images::DEMO, off: &images::DEMO,
},
#[cfg(feature="simulation")]
SensorImage {
source: SensorSource::Simulation,
on: &images::TAPE, off: &images::TAPE
},
];
const DISPLAY_SIZE: Size = Size::new(128, 64);
const SENSOR_ICON_SIZE: u32 = 16;
const SENSOR_ICON_SPACING: u32 = 2;
const SENSOR_TRAY_SIZE: Size = Size::new(SENSOR_IMAGES.len() as u32 * (SENSOR_ICON_SIZE + SENSOR_ICON_SPACING), SENSOR_ICON_SIZE + SENSOR_ICON_SPACING);
const SPEEDO_TRAY_SIZE: Size = Size::new(DISPLAY_SIZE.width - SENSOR_TRAY_SIZE.width, SENSOR_TRAY_SIZE.height);
impl Screen {
pub fn draw_screen<'a, T>(&self, sampler: &mut EmbeddedGraphicsSampler<'a, T>, state: &OledUniforms) where T: Sample<'a, Matrix2DSpace>, T::Output: PixelSink<BinaryColor> {
match self {
@@ -74,36 +159,22 @@ impl Screen {
Screen::Home => {
// Status bar
// Sensor indicators
let gps_img = if state.ui.gps_online {
&images::GPS_ON
} else {
&images::GPS_OFF
};
let imu_img = if state.ui.imu_online {
&images::IMU_ON
} else {
&images::IMU_OFF
};
Image::new(gps_img, Point::zero()).draw(sampler).unwrap();
Image::new(imu_img, Point::new((gps_img.size().width + 2) as i32, 0)).draw(sampler).unwrap();
for (idx, sensor) in SENSOR_IMAGES.iter().enumerate() {
let offset = idx * (16 + 2);
let position = Point::new(offset as i32, 0);
SensorIcon::new(sensor, state.ui.sensor_states[sensor.source], state.frame, position)
.draw(sampler).unwrap();
}
#[cfg(feature="demo")]
Text::with_alignment("Demo", Point::new(128, 10), TEXT_STYLE, Alignment::Right)
.draw(sampler)
.unwrap();
let speedo_center = SENSOR_TRAY_SIZE.width + SPEEDO_TRAY_SIZE.width / 2;
#[cfg(feature="simulation")]
Text::with_alignment("Sim", Point::new(128, 10), TEXT_STYLE, Alignment::Right)
// Speed display at the top
Text::with_alignment(&format!("{}", state.ui.velocity), Point::new(speedo_center as i32, 12), SPEED_STYLE, Alignment::Center)
.draw(sampler)
.unwrap();
// Separates the status bar from the UI
Line::new(Point::new(0, 18), Point::new(128, 18)).draw_styled(&INACTIVE_STYLE, sampler).unwrap();
// Speed display at the top
Text::with_alignment(&format!("{}", state.ui.velocity), Point::new(128 / 2, 12), SPEED_STYLE, Alignment::Center)
.draw(sampler)
.unwrap();
Line::new(Point::new(0, SENSOR_TRAY_SIZE.height as i32), Point::new(128, SENSOR_TRAY_SIZE.height as i32)).draw_styled(&INACTIVE_STYLE, sampler).unwrap();
// The main UI content
Image::new(&images::BIKE, Point::new((128 / 2 - images::BIKE.size().width / 2) as i32, 24)).draw(sampler).unwrap();

14
src/tasks/demo.rs
View File

@@ -1,7 +1,7 @@
use embassy_sync::pubsub::DynPublisher;
use embassy_time::Timer;
use crate::events::{Notification, Scene};
use crate::events::{Notification, Scene, SensorSource, SensorState};
#[embassy_executor::task]
@@ -11,10 +11,16 @@ pub async fn demo_task(ui: DynPublisher<'static, Notification>) {
ui.publish(Notification::SetBrakelight(true)).await;
ui.publish(Notification::SetHeadlight(true)).await;
Timer::after_secs(10).await;
ui.publish(Notification::SensorStatus(SensorSource::Demo, SensorState::AcquiringFix)).await;
loop {
for scene in [Scene::Accelerating, Scene::Ready, Scene::Decelerating, Scene::Ready] {
Timer::after_secs(8).await;
ui.publish(Notification::SceneChange(scene)).await
};
ui.publish(Notification::SceneChange(scene)).await;
for state in [SensorState::Offline, SensorState::AcquiringFix, SensorState::Degraded, SensorState::Offline] {
for sensor in [SensorSource::ForwardsReference, SensorSource::GPS, SensorSource::GravityReference, SensorSource::IMU, SensorSource::Location] {
ui.publish(Notification::SensorStatus(sensor, state)).await;
}
Timer::after_secs(1).await;
}
}
}
}

8
src/tasks/motion.rs
View File

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

5
src/tasks/mpu.rs
View File

@@ -31,7 +31,6 @@ fn gyro_raw_to_rads(raw: i16) -> f32 {
#[esp_hal::ram(rtc_fast, persistent)]
static mut MPU_WAS_CALIBRATED: u8 = 0;
//static mut MPU_CALIBRATION: Option<(Accel, Gyro)> = None;
#[embassy_executor::task]
pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
@@ -39,6 +38,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
let busref = RefCell::new(Some(bus));
backoff.forever().attempt::<_, (), ()>(async || {
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Offline)).await;
let mut sensor = backoff.forever().attempt(async || {
warn!("Initializing connection to MPU");
match Mpu6050::new(busref.replace(None).unwrap(), Address::default()).await.map_err(|e| { e.i2c }) {
@@ -47,6 +47,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
Err(())
},
Ok(mut sensor) => {
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::AcquiringFix)).await;
match backoff.attempt(async || { mpu_init(&mut sensor).await }).await {
Err(_) => {
busref.replace(Some(sensor.release()));
@@ -60,7 +61,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
let sensor_ref = &mut sensor;
events.send(Measurement::SensorOnline(SensorSource::IMU)).await;
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Online)).await;
//TODO: Need to read in a constant buffer of accelerometer measurements, which we can then use to determine where "forward" points in the body frame when converting from the sensor frame.
// From there, we can rotate the body frame into the world frame using gps headings to generate a compass north
fn lowpass(prev: f32, current: f32, alpha: f32) -> f32 {

7
src/tasks/oled.rs
View File

@@ -7,7 +7,7 @@ use figments::{mappings::embedded_graphics::Matrix2DSpace, prelude::{Coordinates
use figments_render::output::{Brightness, OutputAsync};
use log::*;
use crate::{animation::Animation, backoff::Backoff, events::{Notification, Prediction, SensorSource, Telemetry}, graphics::{display::DisplayControls, oled_ui::{OledUniforms, Screen}}};
use crate::{animation::Animation, backoff::Backoff, events::{Notification, Prediction, SensorSource, SensorState, Telemetry}, graphics::{display::DisplayControls, oled_ui::{OledUniforms, Screen}}};
#[cfg(feature="oled")]
pub type OledUiSurfacePool = BufferedSurfacePool<OledUniforms, Matrix2DSpace, BinaryColor>;
@@ -84,10 +84,7 @@ impl<S: core::fmt::Debug + Surface<CoordinateSpace = Matrix2DSpace, Pixel = Bina
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::SensorOffline(SensorSource::IMU)) => self.with_uniforms(|state| {state.ui.imu_online = false}).await,
Telemetry::Notification(Notification::SensorOnline(SensorSource::IMU)) => self.with_uniforms(|state| {state.ui.imu_online = true}).await,
Telemetry::Notification(Notification::SensorOffline(SensorSource::GPS)) => self.with_uniforms(|state| {state.ui.gps_online = false}).await,
Telemetry::Notification(Notification::SensorOnline(SensorSource::GPS)) => self.with_uniforms(|state| {state.ui.gps_online = true}).await,
Telemetry::Notification(Notification::SensorStatus(src, sensor_state)) => self.with_uniforms(|state| {state.ui.sensor_states[src] = sensor_state}).await,
_ => ()
}
}

14
src/tasks/ui.rs
View File

@@ -6,7 +6,7 @@ use core::fmt::Debug;
use futures::join;
use log::*;
use crate::{animation::{AnimatedSurface, Animation}, graphics::display::{SegmentSpace, Uniforms}, events::{Notification, Scene, SensorSource, Telemetry}, graphics::shaders::*};
use crate::{animation::{AnimatedSurface, Animation}, events::{Notification, Scene, SensorSource, SensorState, Telemetry}, graphics::{display::{SegmentSpace, Uniforms}, shaders::*}};
pub struct Ui<S: Surface> {
// Background layer provides an always-running background for everything to draw on
@@ -139,19 +139,13 @@ impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pix
}
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::SensorOnline(SensorSource::IMU) => self.flash_notification_color(Rgb::new(0, 255, 0)).await,
Notification::SensorOffline(SensorSource::GPS) => self.flash_notification_color(Rgb::new(255, 0, 0)).await,
Notification::SensorOnline(SensorSource::GPS) => self.flash_notification_color(Rgb::new(0, 255, 255)).await,
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::SensorsOffline => {
self.flash_notification_color(Rgb::new(255, 0, 0)).await;
self.flash_notification_color(Rgb::new(0, 255, 0)).await;
self.flash_notification_color(Rgb::new(0, 0, 255)).await;
}
Notification::WakeUp => self.show().await,
_ => ()