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

bump a lot of big changes I dont want to break down into individual commits

Browse Source
This commit is contained in:
Victoria Fischer
2025-10-11 16:34:09 +02:00
parent 0e9e0c1b13
commit 8280f38185
48 changed files with 1275467 additions and 394056 deletions
Download Patch File Download Diff File Expand all files Collapse all files

95
src/tasks/mpu.rs
View File

@@ -2,14 +2,15 @@ use core::cell::RefCell;
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::DynamicSender};
use embassy_time::{Delay, Timer};
use embassy_time::{Delay, Timer, Instant};
use esp_hal::{i2c::master::I2c, Async};
use log::{info, warn};
use log::*;
use mpu6050_dmp::{address::Address, calibration::CalibrationParameters, error_async::Error, sensor_async::Mpu6050};
use nalgebra::Vector3;
use core::f32::consts::PI;
use crate::events::SensorSource;
use crate::{backoff::Backoff, ego::tilt::TiltEstimator, events::Measurement};
use crate::{backoff::Backoff, events::Measurement};
const G: f32 = 9.80665;
const SENS_2G: f32 = 16384.0;
@@ -27,43 +28,6 @@ fn gyro_raw_to_rads(raw: i16) -> f32 {
(raw as f32 / 16.4) * (DEG2RAD)
}
/*/// Rotate body accel into ENU and remove gravity; returns (east, north, up) linear accel in m/s^2
pub fn accel_body_to_enu_lin(body_acc: (f32,f32,f32), roll: f32, pitch: f32, yaw: f32) -> (f32,f32,f32) {
let (ax, ay, az) = body_acc;
let (sr, cr) = roll.sin_cos();
let (sp, cp) = pitch.sin_cos();
let (sy, cy) = yaw.sin_cos();
// Build R = Rz(yaw) * Ry(pitch) * Rx(roll)
// multiply R * a_body
let r11 = cy*cp;
let r12 = cy*sp*sr - sy*cr;
let r13 = cy*sp*cr + sy*sr;
let r21 = sy*cp;
let r22 = sy*sp*sr + cy*cr;
let r23 = sy*sp*cr - cy*sr;
let r31 = -sp;
let r32 = cp*sr;
let r33 = cp*cr;
// a_nav = R * a_body
let ax_nav = r11*ax + r12*ay + r13*az; // east
let ay_nav = r21*ax + r22*ay + r23*az; // north
let az_nav = r31*ax + r32*ay + r33*az; // up
// remove gravity (nav frame gravity points down in NED; in our ENU 'up' axis positive up)
// If accel measured gravity as +9.8 on sensor when static and z down convention used above,
// then az_nav will include +g in 'up' direction; subtract G.
let ax_lin = ax_nav;
let ay_lin = ay_nav;
let az_lin = az_nav - G; // linear acceleration in up axis
(ax_lin, ay_lin, az_lin)
}*/
#[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);
@@ -71,7 +35,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
backoff.forever().attempt::<_, (), ()>(async || {
let mut sensor = backoff.forever().attempt(async || {
info!("Initializing MPU");
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));
@@ -91,8 +55,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
let sensor_ref = &mut sensor;
info!("MPU is ready!");
let _tilt = TiltEstimator::new(0.3);
events.send(Measurement::SensorOnline(SensorSource::IMU)).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 {
@@ -122,12 +85,6 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
);
prev_accel = adjusted;
// Apply current rotation and accel values to rotate our values back into the ENU frame
/*tilt.update(0.01, (filtered.x, filtered.y, filtered.z), (adjusted_gyro.x, adjusted_gyro.y, adjusted_gyro.z));
let (roll, pitch, yaw) = tilt.angles();
let (ax_e, ay_n, az_up) = accel_body_to_enu_lin((filtered.x,filtered.y,filtered.z), roll, pitch, yaw);
let measured = Vector3::new(ax_e, ay_n, az_up);*/
events.send(
Measurement::IMU {
accel: filtered,
@@ -137,7 +94,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
Timer::after_millis(10).await;
},
Err(e) => {
warn!("Failed to read MPU motion data! {e:?}");
error!("Failed to read MPU motion data! {e:?}");
busref.replace(Some(sensor.release()));
return Err(());
}
@@ -147,18 +104,28 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
}
async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>) -> Result<(), Error<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>> {
let mut delay = Delay;
let backoff = Backoff::from_millis(10);
info!("Initializing MPU");
backoff.attempt(async || { sensor.initialize_dmp(&mut delay).await }).await?;
backoff.attempt(async || { sensor.set_digital_lowpass_filter(mpu6050_dmp::config::DigitalLowPassFilter::Filter3).await }).await?;
info!("Calibrating MPU");
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.map(|_| { Ok(()) })
}).await?
if cfg!(feature="wokwi") {
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());
Ok(())
}
}