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

src: implement simulation data sources

Browse Source
This commit is contained in:
Victoria Fischer
2025-09-24 08:32:34 +02:00
parent 19875f6ae5
commit 0cd2cc94b9
32 changed files with 389785 additions and 284 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
src/tasks/mpu.rs
View File

@@ -1,16 +1,15 @@
use core::cell::RefCell;
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::{DynamicSender, Sender}};
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::DynamicSender};
use embassy_time::{Delay, Timer};
use esp_hal::{i2c::master::I2c, Async};
use log::{info, warn};
use mpu6050_dmp::{address::Address, calibration::CalibrationParameters, error_async::Error, sensor_async::Mpu6050};
use nalgebra::Vector3;
use core::f32::consts::PI;
use nalgebra::ComplexField;
use crate::{backoff::Backoff, ego::{alignment::SensorAlignment, tilt::TiltEstimator}, events::{Measurement, Notification}};
use crate::{backoff::Backoff, ego::tilt::TiltEstimator, events::Measurement};
const G: f32 = 9.80665;
const SENS_2G: f32 = 16384.0;
@@ -28,7 +27,7 @@ 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
/*/// 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;
@@ -63,10 +62,10 @@ pub fn accel_body_to_enu_lin(body_acc: (f32,f32,f32), roll: f32, pitch: f32, yaw
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: Sender<'static, NoopRawMutex, Measurement, 4>, sink: DynamicSender<'static, Notification>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
let backoff = Backoff::from_millis(5);
let busref = RefCell::new(Some(bus));
@@ -92,24 +91,8 @@ pub async fn mpu_task(events: Sender<'static, NoopRawMutex, Measurement, 4>, sin
let sensor_ref = &mut sensor;
let alignment = {
info!("Locating earth...");
let mut accel_samples = [Vector3::default(); 100];
let mut gyro_samples = [Vector3::default(); 100];
for (idx, (accel_sample, gyro_sample)) in accel_samples.iter_mut().zip(gyro_samples.iter_mut()).enumerate() {
let (accel, gyro) = backoff.attempt(async || { sensor_ref.motion6().await }).await.unwrap();
*accel_sample = Vector3::new(accel_raw_to_ms2(accel.x()), accel_raw_to_ms2(accel.y()), accel_raw_to_ms2(accel.z()));
*gyro_sample = Vector3::new(gyro_raw_to_rads(gyro.x()), gyro_raw_to_rads(gyro.y()), gyro_raw_to_rads(gyro.z()));
info!("{idx}/200 {accel_sample:?} {gyro_sample:?}");
Timer::after_millis(5).await;
}
SensorAlignment::new(accel_samples.as_slice(), gyro_samples.as_slice())
};
info!("MPU is ready! earth={alignment:?}");
sink.send(Notification::CalibrationComplete).await;
let mut tilt = TiltEstimator::new(0.3);
info!("MPU is ready!");
let _tilt = TiltEstimator::new(0.3);
//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 {
@@ -121,16 +104,16 @@ pub async fn mpu_task(events: Sender<'static, NoopRawMutex, Measurement, 4>, sin
Ok((accel_data, gyro_data)) => {
// Apply the initial alignment correction to put it into the body frame where X is forward
let adjusted = alignment.apply(&Vector3::new(
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())
));
let adjusted_gyro = alignment.apply_gyro(&Vector3::new(
);
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())
));
);
let filtered = Vector3::new(
lowpass(prev_accel.x, adjusted.x, 0.3),
@@ -140,15 +123,16 @@ pub async fn mpu_task(events: Sender<'static, NoopRawMutex, Measurement, 4>, sin
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));
/*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 (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, adjusted_gyro.z);
let measured = Vector3::new(ax_e, ay_n, az_up);*/
events.send(
Measurement::IMU(
measured
)
Measurement::IMU {
accel: filtered,
gyro: adjusted_gyro
}
).await;
Timer::after_millis(10).await;
},