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()
}
}
}