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

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

Browse Source
This commit is contained in:
Victoria Fischer
2025-12-24 09:15:18 +01:00
parent 7cc87b42c8
commit 81df3e2973
1 changed files with 21 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
src/tasks/mpu.rs
View File

@@ -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(
@@ -114,49 +102,13 @@ async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, NoopRawMutex, I2c<'sta
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);
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(())
}
}