From c747a0fbf1b0aee1158a147bb59cdfde2a054570 Mon Sep 17 00:00:00 2001
From: Victoria Fischer <tdfischer@hackerbots.net>
Date: Fri, 27 Mar 2026 22:53:37 +0100
Subject: [PATCH] tasks: mpu: rewrite to drop usage of backoff

---
 src/tasks/mpu.rs | 70 +++++++++++++++++++++++++-----------------------
 1 file changed, 37 insertions(+), 33 deletions(-)

diff --git a/src/tasks/mpu.rs b/src/tasks/mpu.rs
index 870bde1..9614448 100644
--- a/src/tasks/mpu.rs
+++ b/src/tasks/mpu.rs
@@ -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(())
     }