From 81df3e29730d329cac280f112a0eff37755e6b4b Mon Sep 17 00:00:00 2001
From: Victoria Fischer <tdfischer@hackerbots.net>
Date: Wed, 24 Dec 2025 09:15:18 +0100
Subject: [PATCH] tasks: mpu: remove the whole calibration step, as this is now
 handled inside the motion engine. maybe in the future we rewrite again to
 utilize the full DMP FIFO system on the IMU chip

---
 src/tasks/mpu.rs | 90 +++++++++++-------------------------------------
 1 file changed, 21 insertions(+), 69 deletions(-)

diff --git a/src/tasks/mpu.rs b/src/tasks/mpu.rs
index a423ebb..cfe8f12 100644
--- a/src/tasks/mpu.rs
+++ b/src/tasks/mpu.rs
@@ -6,32 +6,17 @@ use embassy_sync::channel::DynamicSender;
 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;
+use mpu6050_dmp::accel::AccelFullScale;
+use mpu6050_dmp::gyro::GyroFullScale;
+use mpu6050_dmp::{address::Address, error_async::Error, sensor_async::Mpu6050};
 use nalgebra::Vector3;
-use core::f32::consts::PI;
 use crate::events::SensorSource;
 
 use crate::{backoff::Backoff, events::Measurement};
 
 const G: f32 = 9.80665;
-const SENS_2G: f32 = 16384.0;
-const DEG2RAD: f32 = PI / 180.0;
-
-/// Convert raw accel (i16) to m/s^2
-fn accel_raw_to_ms2(raw: i16) -> f32 {
-    (raw as f32 / SENS_2G) * G
-}
-
-/// Convert raw gyro (i16) to rad/s. For MPU6050 typical gyro scale at ±2000 deg/s -> 16.4 LSB/deg/s
-/// Adjust if you configured different full-scale range.
-fn gyro_raw_to_rads(raw: i16) -> f32 {
-    // lsb_per_dps e.g. 16.4 for ±2000 dps
-    (raw as f32 / 16.4) * (DEG2RAD)
-}
-
-#[esp_hal::ram(unstable(rtc_fast, persistent))]
-static mut MPU_WAS_CALIBRATED: u8 = 0;
+const GYRO_SCALE: GyroFullScale = GyroFullScale::Deg2000;
+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>>) {
@@ -74,15 +59,18 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
                 Ok((accel_data, gyro_data)) => {
 
                     // 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(
-                        accel_raw_to_ms2(accel_data.x()),
-                        accel_raw_to_ms2(accel_data.y()),
-                        accel_raw_to_ms2(accel_data.z())
-                    );
+                        scaled.x(),
+                        scaled.y(),
+                        scaled.z()
+                    ) * G;
+
+                    let scaled = gyro_data.scaled(GYRO_SCALE);
                     let adjusted_gyro = Vector3::new(
-                        gyro_raw_to_rads(gyro_data.x()),
-                        gyro_raw_to_rads(gyro_data.y()),
-                        gyro_raw_to_rads(gyro_data.z())
+                        scaled.x(),
+                        scaled.y(),
+                        scaled.z()
                     );
 
                     let filtered = Vector3::new(
@@ -115,48 +103,12 @@ async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, NoopRawMutex, I2c<'sta
         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);
-            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());
-        }
+        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?;
+        info!("DMP initialized in {}ms", start.as_millis());
         Ok(())
     }
 }
\ No newline at end of file