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

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::<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
+                // We only want to wake up from sleep if our current velocity is something like a bump
-                if mean > 0.5 {
+                if average_speed > BUMP_SPEED_NOISE_GATE && self.sleep_timer.wake() {
-                    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
+                // Here, we additionally release the parking brake if we are currently parked and reading substantial movement
-                    if self.parking_timer.wake() {
+                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);
                 }