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

events: combine gravity and forwards fusion sensors into a shared motionframe

Browse Source
This commit is contained in:
Victoria Fischer
2026-02-28 16:52:00 +01:00
parent f2b2674158
commit 935f30d968
4 changed files with 9 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/ego/engine.rs
View File

@@ -28,8 +28,7 @@ pub struct BikeStates {
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
has_down: Breaker<bool>,
has_forwards: Breaker<bool>,
has_motion_frame: Breaker<bool>,
motion_state: Breaker<MotionState>,
acquiring_data: Breaker<bool>,
// 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(),

2
src/ego/orientation.rs
View File

@@ -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);
}