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

tasks: mpu: rewrite to drop usage of backoff

Browse Source
This commit is contained in:
Victoria Fischer
2026-03-27 22:53:37 +01:00
parent fc9bd0b7a7
commit c747a0fbf1
1 changed files with 37 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
src/tasks/mpu.rs
View File

@@ -1,5 +1,3 @@
use core::cell::RefCell;
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
use embassy_sync::blocking_mutex::raw::NoopRawMutex;
use embassy_sync::channel::DynamicSender;
@@ -13,7 +11,7 @@ use nalgebra::Vector3;
use crate::events::SensorSource;
use crate::gpio_interrupt::PinInterrupt;
use crate::{backoff::Backoff, events::Measurement};
use crate::events::Measurement;
const G: f32 = 9.80665;
const GYRO_SCALE: GyroFullScale = GyroFullScale::Deg2000;
@@ -21,44 +19,44 @@ const ACCEL_SCALE: AccelFullScale = AccelFullScale::G2;
#[embassy_executor::task]
pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevice<'static, NoopRawMutex, I2c<'static, Async>>, _interrupt: PinInterrupt<'static>) {
let backoff = Backoff::from_millis(5);
let busref = RefCell::new(Some(bus));
warn!("Initializing connection to MPU");
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::AcquiringFix)).await;
backoff.forever().attempt::<_, (), ()>(async || {
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Offline)).await;
let mut sensor = backoff.forever().attempt(async || {
warn!("Initializing connection to MPU");
match Mpu6050::new(busref.replace(None).unwrap(), Address::default()).await.map_err(|e| { e.i2c }) {
Err(i2c) => {
busref.replace(Some(i2c));
Err(())
},
Ok(mut sensor) => {
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::AcquiringFix)).await;
match backoff.attempt(async || { mpu_init(&mut sensor).await }).await {
Err(_) => {
busref.replace(Some(sensor.release()));
Err(())
},
Ok(_) => Ok(sensor)
}
}
}
}).await?;
let mut sensor = match Mpu6050::new(bus, Address::default()).await {
Err(err) => {
error!("Could not connect to MPU: {err:?}");
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Offline)).await;
return;
},
Ok(sensor) => sensor
};
let sensor_ref = &mut sensor;
loop {
if let Err(err) = mpu_init(&mut sensor).await {
error!("Could not initialize MPU: {err:?}");
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Degraded)).await;
continue;
}
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Online)).await;
//TODO: Need to read in a constant buffer of accelerometer measurements, which we can then use to determine where "forward" points in the body frame when converting from the sensor frame.
// From there, we can rotate the body frame into the world frame using gps headings to generate a compass north
fn lowpass(prev: f32, current: f32, alpha: f32) -> f32 {
prev + alpha * (current - prev) // alpha in (0,1), small alpha = heavy smoothing
}
let mut prev_accel = Vector3::default();
let mut degraded = false;
loop {
match backoff.attempt(async || { sensor_ref.motion6().await }).await {
match sensor.motion6().await {
Ok((accel_data, gyro_data)) => {
if degraded {
warn!("Reconnected to MPU");
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Online)).await;
degraded = false;
}
// Apply the initial alignment correction to put it into the body frame where X is forward
let scaled = accel_data.scaled(ACCEL_SCALE);
let adjusted = Vector3::new(
@@ -91,12 +89,19 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
},
Err(e) => {
error!("Failed to read MPU motion data! {e:?}");
busref.replace(Some(sensor.release()));
return Err(());
if !degraded {
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::Degraded)).await;
degraded = true;
} else {
error!("MPU connection is degraded, and failed to read data again. Attempting to re-initialize connection...");
events.send(Measurement::SensorHardwareStatus(SensorSource::IMU, crate::events::SensorState::AcquiringFix)).await;
break;
}
Timer::after_millis(10).await;
}
};
}
}).await.ok();
}
}
async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, NoopRawMutex, I2c<'static, Async>>>) -> Result<(), Error<I2cDevice<'static, NoopRawMutex, I2c<'static, Async>>>> {
@@ -105,10 +110,9 @@ async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, NoopRawMutex, I2c<'sta
Ok(())
} else {
let mut delay = Delay;
let backoff = Backoff::from_millis(10);
info!("Initializing DMP");
let start = Instant::now();
backoff.attempt(async || { sensor.initialize_dmp(&mut delay).await }).await?;
sensor.initialize_dmp(&mut delay).await?;
info!("DMP initialized in {}ms", start.as_millis());
Ok(())
}