bump a lot of big changes I dont want to break down into individual commits

src/ego/engine.rs

@@ -0,0 +1,264 @@
+use embassy_sync::channel::DynamicSender;
+use embassy_time::{Duration, Instant};
+use nalgebra::{Rotation3, Vector2, Vector3};
+use log::*;
+//use micromath::F32Ext;
+use nalgebra::{ComplexField, RealField};
+
+use core::fmt::Debug;
+
+use crate::{ego::{heading::HeadingEstimator, kalman::Ekf2D, orientation::OrientationEstimator}, events::{Notification, Prediction}, Breaker, CircularBuffer, idle::IdleClock};
+
+#[derive(PartialEq, Debug, Default, Clone, Copy)]
+pub enum MotionState {
+    #[default]
+    Stationary,
+    Steady,
+    Accelerating,
+    Decelerating
+}
+
+pub struct BikeStates {
+    /// Prediction models
+    orientation: OrientationEstimator,
+    heading: HeadingEstimator,
+    speedo: CircularBuffer<f32, 300>,
+    kf: Ekf2D,
+    
+    last_stamp: Instant,
+    last_fix: Vector2<f64>, // The most recent GPS value
+    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>,
+    motion_state: Breaker<MotionState>,
+    // FIXME: pub
+    pub has_gps_fix: Breaker<bool>,
+    predicted_velocity: Breaker<f32>,
+    reported_velocity: Breaker<f32>,
+    predicted_location: Breaker<Vector2<f64>>,
+    wake_requested: Breaker<bool>,
+    steady_timer: IdleClock
+}
+
+impl Debug for BikeStates {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        f.debug_struct("BikeStates")
+            .field("has_down", &self.orientation.has_down())
+            .field("has_orientation", &self.orientation.is_ready())
+            .field("heading", &self.heading.heading())
+            .field("motion_state", &self.motion_state)
+            .field("predicted_location", &self.predicted_location)
+            .field("predicted_velocity", &self.predicted_velocity)
+            .field("wake_requested", &self.wake_requested)
+            .finish()
+    }
+}
+
+impl BikeStates {
+    pub fn insert_gps(&mut self, gps_pos: Vector2<f64>) {
+        match self.reference_fix {
+            None => {
+                self.reference_fix = Some(gps_pos);
+                self.last_fix = gps_pos;
+                self.has_gps_fix.set(true);
+            },
+            Some(coords) => {
+                if self.last_fix != coords {
+                    let gps_heading = self.last_fix.angle(&coords);
+                    self.heading.correct(gps_heading as f32, 0.9);
+                }
+
+                let delta = gps_to_local_meters_haversine(&coords, &gps_pos);
+
+                self.kf.update_gps(delta.x, delta.y);
+
+                self.last_fix = coords;
+            }
+        }
+    }
+
+    pub fn insert_imu(&mut self, accel: Vector3<f32>, gyro: Vector3<f32>) {
+        self.orientation.insert(accel);
+
+        if self.orientation.has_down() {
+            let body_accel = self.orientation.apply(accel);
+            let body_gyro = self.orientation.apply(gyro);
+
+            let cur_stamp = Instant::now();
+            let dt = (cur_stamp - self.last_stamp).as_millis() as f32 / 1000.0;
+            self.last_stamp = cur_stamp;
+
+            self.heading.update(body_gyro.z, dt);
+            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 {
+                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
+                self.kf.update_zupt();
+            }
+
+            self.is_calibrated.set(true);
+        }
+    }
+
+    pub async fn commit(&mut self, predictions: &DynamicSender<'static, Prediction>, notifications: &DynamicSender<'static, Notification>) {
+        if let Some(true) = self.is_calibrated.read_tripped() {
+            notifications.send(Notification::SensorOnline(crate::events::SensorSource::IMU)).await
+        }
+
+        match self.has_gps_fix.read_tripped() {
+            None => (),
+            Some(true) => notifications.send(Notification::SensorOnline(crate::events::SensorSource::GPS)).await,
+            Some(false) => notifications.send(Notification::SensorOffline(crate::events::SensorSource::GPS)).await,
+        }
+
+        let est = self.kf.x;
+
+        let position = self.reference_fix.as_ref().map(|start_coords| local_to_gps(*start_coords, Vector2::new(est.x as f64, est.y as f64)));
+        let velocity = Vector2::new(est[2], est[3]);
+
+        // If we have a new location, update the predictions
+        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;
+            }
+        }
+
+        // If we have a new 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 self.speedo.is_filled() {
+                let threshold = 1.0;
+                let trend = self.speedo.data().windows(2).map(|n| {
+                    n[1] - n[0]
+                }).sum::<f32>();
+                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;
+                }
+
+                // If the total slope is more upwards than not, we are accelerating.
+                if trend >= threshold {
+                    self.motion_state.set(MotionState::Accelerating);
+                    self.steady_timer.wake();
+                } else if trend <= -threshold {
+                    self.steady_timer.wake();
+                    self.motion_state.set(MotionState::Decelerating);
+                } 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 {
+                    // 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);
+        }
+    }
+}
+
+impl Default for BikeStates {
+    fn default() -> Self {
+        Self {
+            motion_state: Default::default(),
+            orientation: Default::default(),
+            last_stamp: Instant::now(),
+            speedo: Default::default(),
+            heading: Default::default(),
+            is_calibrated: Default::default(),
+            kf: Default::default(),
+            steady_timer: IdleClock::new(Duration::from_secs(3)),
+            last_fix: Default::default(),
+            reference_fix: Default::default(),
+            has_gps_fix: Default::default(),
+            predicted_location: Default::default(),
+            predicted_velocity: Default::default(),
+            reported_velocity: Default::default(),
+            wake_requested: Default::default()
+        }
+    }
+}
+
+/// Compute displacement vector (east, north) in meters between two GPS coordinates
+/// Input: Vector2::new(lat_deg, lon_deg)
+pub fn gps_to_local_meters_haversine(a: &Vector2<f64>, b: &Vector2<f64>) -> Vector2<f32> {
+    let r = 6_371_000.0; // Earth radius in meters
+
+    // Convert to radians
+    let lat1 = a.x.to_radians();
+    let lon1 = a.y.to_radians();
+    let lat2 = b.x.to_radians();
+    let lon2 = b.y.to_radians();
+
+    // Differences
+    let dlat = lat2 - lat1;
+    let dlon = lon2 - lon1;
+
+    // Haversine formula for great-circle distance
+    let hav = (dlat / 2.0).sin().powi(2)
+        + lat1.cos() * lat2.cos() * (dlon / 2.0).sin().powi(2);
+    let c = 2.0 * hav.sqrt().atan2((1.0 - hav).sqrt());
+    let distance = r * c;
+
+    // Bearing from point A to B
+    let y = dlon.sin() * lat2.cos();
+    let x = lat1.cos() * lat2.sin() - lat1.sin() * lat2.cos() * dlon.cos();
+    let bearing = y.atan2(x); // radians, from north, clockwise
+
+    // Convert to local east/north vector
+    let dx = distance * bearing.sin(); // east
+    let dy = distance * bearing.cos(); // north
+
+    Vector2::new(dx as f32, dy as f32)
+}
+
+/// Convert a local displacement (east, north) in meters back to GPS coordinates.
+/// Input:
+///   ref_gps = Vector2::new(lat_deg, lon_deg)
+///   disp    = Vector2::new(dx_east_m, dy_north_m)
+/// Output:
+///   Vector2::new(lat_deg, lon_deg)
+pub fn local_to_gps(ref_gps: Vector2<f64>, disp: Vector2<f64>) -> Vector2<f64> {
+    let r = 6_371_000.0; // Earth radius in meters
+
+    let lat0 = ref_gps.x.to_radians();
+    let lon0 = ref_gps.y.to_radians();
+
+    // Displacement in meters
+    let dx = disp.x; // east
+    let dy = disp.y; // north
+
+    // Distance and bearing
+    let distance = (dx.powi(2) + dy.powi(2)).sqrt();
+    let bearing = dx.atan2(dy); // atan2(east, north)
+
+    // Destination point on sphere
+    let lat = (lat0.sin() * (distance / r).cos()
+        + lat0.cos() * (distance / r).sin() * bearing.cos())
+        .asin();
+
+    let lon = lon0
+        + (bearing.sin() * (distance / r).sin() * lat0.cos())
+            .atan2((distance / r).cos() - lat0.sin() * lat.sin());
+
+    Vector2::new(lat.to_degrees(), lon.to_degrees())
+}