From 315fa633c19156ecc4ecab438b35aeb364f1e246 Mon Sep 17 00:00:00 2001 From: Victoria Fischer Date: Sat, 28 Feb 2026 17:15:32 +0100 Subject: [PATCH] ego: engine: move some of the system constants out into real const values --- src/ego/engine.rs | 58 ++++++++++++++++++++++++++++++----------------- 1 file changed, 37 insertions(+), 21 deletions(-) diff --git a/src/ego/engine.rs b/src/ego/engine.rs index aca03a7..951bb99 100644 --- a/src/ego/engine.rs +++ b/src/ego/engine.rs @@ -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::(); // 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); }