mpu: attempt to avoid recalibration after a basic reboot, if we were calibrated prior to the reboot

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);
@@ -107,6 +117,13 @@ async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, CriticalSectionRawMute
     if cfg!(feature="wokwi") {
         warn!("Initializing simulated MPU");
         Ok(())
+    } else {
+        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);
@@ -114,18 +131,35 @@ async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, CriticalSectionRawMute
             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");
+            info!("DMP initialized in {}ms", start.as_millis());
+            warn!("Calibrating MPU from scratch");
             let start = Instant::now();
-        backoff.attempt(async || {
+            let calibration_data = backoff.attempt(async || {
                 // TODO: Store calibration data with set_accel_calibration, set_gyro_calibration
-            sensor.calibrate(&mut delay, &CalibrationParameters::new(
+                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
-            )).await
+                );
+                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(())
     }
 }