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

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