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

ego: engine: move some of the system constants out into real const values

Browse Source
This commit is contained in:
Victoria Fischer
2026-02-28 17:15:32 +01:00
parent 54ebbbaea7
commit 315fa633c1
1 changed files with 37 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
src/ego/engine.rs
View File

@@ -42,6 +42,24 @@ pub struct BikeStates {
sleep_timer: IdleClock,
}
/// Above this speed, the system should consider waking up from sleep mode
const BUMP_SPEED_NOISE_GATE: f32 = 0.1;
/// Above this speed, the system should be fully awake and ready to start moving around with purpose
const WAKEUP_SPEED_NOISE_GATE: f32 = 0.5;
/// Above this average speed, we are considered to be moving around with purpose
const MOVING_SPEED_NOISE_GATE: f32 = 1.0;
/// IMU readings of at least this value are considered valid motion. Below this value, the signal is considered noise while stationary
const MOTION_NOISE_GATE: f32 = 0.8;
/// If the system's calculated speed is increasing or decreasing by this value or more, we are accelerating/decelerating
const ACCELERATION_NOISE_GATE: f32 = 1.0;
/// When we get a heading via GPS, this determines how much weight that value has when blended into the system.
const GPS_HEADING_ALPHA_CORRECTION: f32 = 0.9;
impl Debug for BikeStates {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
f.debug_struct("BikeStates")
@@ -69,7 +87,7 @@ impl BikeStates {
Some(coords) => {
if self.last_fix != coords {
let gps_heading = self.last_fix.angle(&coords);
self.heading.correct(gps_heading as f32, 0.9);
self.heading.correct(gps_heading as f32, GPS_HEADING_ALPHA_CORRECTION);
}
let delta = gps_to_local_meters_haversine(&coords, &gps_pos);
@@ -97,7 +115,7 @@ impl BikeStates {
let heading_rotation = Rotation3::from_axis_angle(&Vector3::z_axis(), self.heading.heading());
let enu_rotated = heading_rotation * body_accel;
if body_accel.xy().magnitude() >= 0.8 {
if body_accel.xy().magnitude() >= MOTION_NOISE_GATE {
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
@@ -148,45 +166,43 @@ impl BikeStates {
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();
let average_speed = self.speedo.mean();
info!("prediction={current_prediction:?} mean={average_speed}");
// 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);
self.reported_velocity.set((average_speed * 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;
}
// We only want to wake up from sleep if our current velocity is something like a bump
if average_speed > BUMP_SPEED_NOISE_GATE && 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 substantial movement
if average_speed > WAKEUP_SPEED_NOISE_GATE && 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.
if trend >= threshold {
if trend >= ACCELERATION_NOISE_GATE {
self.motion_state.set(MotionState::Accelerating);
self.steady_timer.wake();
} else if trend <= -threshold {
} else if trend <= -ACCELERATION_NOISE_GATE {
self.steady_timer.wake();
self.motion_state.set(MotionState::Decelerating);
} else if self.steady_timer.check() && mean > 1.0 {
} else if self.steady_timer.check() && average_speed > MOVING_SPEED_NOISE_GATE {
// 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 current_prediction <= 1.0 && mean <= 1.0 {
} else if current_prediction <= 1.0 && average_speed <= MOVING_SPEED_NOISE_GATE {
// If the average and instantaneous speed is rather low, we are probably stationary!
self.motion_state.set(MotionState::Stationary);
}