From 9f17c6a8efca767961b985e66116d9a68b40b93c Mon Sep 17 00:00:00 2001
From: Victoria Fischer <tdfischer@hackerbots.net>
Date: Fri, 17 Oct 2025 14:40:26 +0200
Subject: [PATCH] mpu: attempt to avoid recalibration after a basic reboot, if
 we were calibrated prior to the reboot

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

diff --git a/src/tasks/mpu.rs b/src/tasks/mpu.rs
index 49ac07f..a3e2ed4 100644
--- a/src/tasks/mpu.rs
+++ b/src/tasks/mpu.rs
@@ -1,4 +1,5 @@
 use core::cell::RefCell;
+use core::u8;
 
 use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
 use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::DynamicSender};
@@ -6,6 +7,11 @@ use embassy_time::{Delay, Timer, Instant};
 use esp_hal::{i2c::master::I2c, Async};
 use log::*;
 use mpu6050_dmp::{address::Address, calibration::CalibrationParameters, error_async::Error, sensor_async::Mpu6050};
+use mpu6050_dmp::{
+    calibration::CalibrationActions,
+    accel::Accel,
+    gyro::Gyro
+};
 use nalgebra::Vector3;
 use core::f32::consts::PI;
 use crate::events::SensorSource;
@@ -28,6 +34,10 @@ fn gyro_raw_to_rads(raw: i16) -> f32 {
     (raw as f32 / 16.4) * (DEG2RAD)
 }
 
+#[esp_hal::ram(rtc_fast, persistent)]
+static mut MPU_WAS_CALIBRATED: u8 = 0;
+//static mut MPU_CALIBRATION: Option<(Accel, Gyro)> = None;
+
 #[embassy_executor::task]
 pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
     let backoff = Backoff::from_millis(5);
@@ -108,24 +118,48 @@ async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, CriticalSectionRawMute
         warn!("Initializing simulated MPU");
         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?;
-        backoff.attempt(async || { sensor.set_digital_lowpass_filter(mpu6050_dmp::config::DigitalLowPassFilter::Filter1).await }).await?;
-        info!("DMP ready in {}ms", start.as_millis());
-        info!("Calibrating MPU");
-        let start = Instant::now();
-        backoff.attempt(async || {
-            // TODO: Store calibration data with set_accel_calibration, set_gyro_calibration
-            sensor.calibrate(&mut delay, &CalibrationParameters::new(
-                mpu6050_dmp::accel::AccelFullScale::G2,
-                mpu6050_dmp::gyro::GyroFullScale::Deg2000,
-                mpu6050_dmp::calibration::ReferenceGravity::Zero
-            )).await
-        }).await?;
-        info!("MPU calibrated in {}ms", start.as_millis());
+        let calibration_data = sensor.get_accel_calibration().await;
+        info!("Calibration data: {calibration_data:?}");
+        if unsafe { MPU_WAS_CALIBRATED } == u8::MAX {
+            warn!("Re-using MPU calibration data from last boot");
+            //sensor.set_accel_calibration(&accel).await?;
+            //sensor.set_gyro_calibration(&gyro).await?;
+        } 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?;
+            backoff.attempt(async || { sensor.set_digital_lowpass_filter(mpu6050_dmp::config::DigitalLowPassFilter::Filter1).await }).await?;
+            info!("DMP initialized in {}ms", start.as_millis());
+            warn!("Calibrating MPU from scratch");
+            let start = Instant::now();
+            let calibration_data = backoff.attempt(async || {
+                // TODO: Store calibration data with set_accel_calibration, set_gyro_calibration
+                let mut actions = CalibrationActions::all();
+                let mut accel;
+                let mut gyro;
+                let params = CalibrationParameters::new(
+                    mpu6050_dmp::accel::AccelFullScale::G2,
+                    mpu6050_dmp::gyro::GyroFullScale::Deg2000,
+                    mpu6050_dmp::calibration::ReferenceGravity::Zero
+                );
+                loop {
+                    (actions, accel, gyro) = sensor.calibration_loop(&mut delay, &params, actions).await?;
+                    if actions.is_empty() {
+                        return Ok((accel, gyro))
+                    }
+                }
+                /*sensor.calibrate(&mut delay, &CalibrationParameters::new(
+                    mpu6050_dmp::accel::AccelFullScale::G2,
+                    mpu6050_dmp::gyro::GyroFullScale::Deg2000,
+                    mpu6050_dmp::calibration::ReferenceGravity::Zero
+                )).await*/
+            }).await?;
+            //unsafe { MPU_CALIBRATION = Some(calibration_data) };
+            unsafe { MPU_WAS_CALIBRATED = u8::MAX };
+            info!("MPU calibrated in {}ms", start.as_millis());
+        }
         Ok(())
     }
 }
\ No newline at end of file