diff --git a/src/ego/engine.rs b/src/ego/engine.rs index c867e9d..aca03a7 100644 --- a/src/ego/engine.rs +++ b/src/ego/engine.rs @@ -28,8 +28,7 @@ pub struct BikeStates { reference_fix: Option>, // 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 - has_down: Breaker, - has_forwards: Breaker, + has_motion_frame: Breaker, motion_state: Breaker, acquiring_data: Breaker, // FIXME: pub @@ -46,7 +45,6 @@ pub struct BikeStates { 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) @@ -106,10 +104,8 @@ impl BikeStates { self.kf.update_zupt(); } - self.has_down.set(true); - if self.orientation.is_ready() { - self.has_forwards.set(true); + self.has_motion_frame.set(true); } } } @@ -118,17 +114,12 @@ impl BikeStates { let last_motion = self.motion_state.value; if let Some(true) = self.acquiring_data.read_tripped() { - predictions.publish(Prediction::SensorStatus(SensorSource::ForwardsReference, SensorState::AcquiringFix)).await; - predictions.publish(Prediction::SensorStatus(SensorSource::GravityReference, SensorState::AcquiringFix)).await; + predictions.publish(Prediction::SensorStatus(SensorSource::MotionFrame, SensorState::AcquiringFix)).await; predictions.publish(Prediction::SensorStatus(SensorSource::Location, SensorState::AcquiringFix)).await; } - if let Some(true) = self.has_down.read_tripped() { - predictions.publish(Prediction::SensorStatus(SensorSource::GravityReference, SensorState::Online)).await - } - - if let Some(true) = self.has_forwards.read_tripped() { - predictions.publish(Prediction::SensorStatus(SensorSource::ForwardsReference, SensorState::Online)).await + if let Some(true) = self.has_motion_frame.read_tripped() { + predictions.publish(Prediction::SensorStatus(SensorSource::MotionFrame, SensorState::Online)).await } match self.has_gps_fix.read_tripped() { @@ -232,8 +223,7 @@ impl Default for BikeStates { last_stamp: Instant::now(), speedo: Default::default(), heading: Default::default(), - has_down: Default::default(), - has_forwards: Default::default(), + has_motion_frame: Default::default(), kf: Default::default(), steady_timer: IdleClock::new(Duration::from_secs(3)), last_fix: Default::default(), diff --git a/src/ego/orientation.rs b/src/ego/orientation.rs index dd52431..f015fe2 100644 --- a/src/ego/orientation.rs +++ b/src/ego/orientation.rs @@ -66,7 +66,7 @@ impl OrientationEstimator { self.forward = Some(Unit::new_unchecked(-unit_forward.into_inner())); } } else { - info!("Found forwards: {unit_forward:?} avg.norm={}", horiz.norm()); + info!("Motion frame established with forwards={unit_forward:?} avg.norm={} down={:?} bias={:?}", horiz.norm(), self.down, self.sensor_bias); self.forward = Some(unit_forward); } diff --git a/src/events.rs b/src/events.rs index a99ce93..e104a95 100644 --- a/src/events.rs +++ b/src/events.rs @@ -78,8 +78,7 @@ pub enum SensorSource { Wifi, // Fusion outputs - GravityReference, - ForwardsReference, + MotionFrame, Location, Cloud, diff --git a/src/graphics/oled_ui.rs b/src/graphics/oled_ui.rs index 160c178..a41e0f8 100644 --- a/src/graphics/oled_ui.rs +++ b/src/graphics/oled_ui.rs @@ -108,7 +108,7 @@ const SENSOR_IMAGES: &[SensorImage] = &[ on: &images::IMU_ON, off: &images::IMU_OFF, }, SensorImage { - source: SensorSource::GravityReference, + source: SensorSource::MotionFrame, on: &images::GRAVITY_LOCATED, off: &images::GRAVITY_MISSING, }, SensorImage {