I think lazy_lock isn't suitable for my situation, since you'd need access to the IMU within the closure. Unless I'm missing something? I'll include a snippet of the code so you can understand the dilemma.
use defmt::info;
use embassy_stm32::i2c::{I2c, Master};
use embassy_stm32::mode::Async;
use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_sync::signal::Signal;
use embassy_time::{Delay, Duration, Ticker, Timer};
use icm20948_async::{AccDlp, AccRange, AccUnit, GyrDlp, GyrRange, GyrUnit, IcmBuilder};
use static_cell::StaticCell;
use crate::config::{IMU_CALC_STATIC_BIAS_SAMPLES, IMU_READ_TICK_RATE};
#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub struct ImuData<T> {
pub acc: [T; 3],
pub gyro: [T; 3],
pub temp: T,
}
static IMU_STATIC_BIAS: StaticCell<ImuData<f32>> = StaticCell::new();
#[embassy_executor::task]
pub async fn imu_task(
i2c: I2c<'static, Async, Master>,
signal: &'static Signal<ThreadModeRawMutex, ImuData<f32>>,
) {
info!("IMU START");
let mut imu = IcmBuilder::new_i2c(i2c, Delay)
.gyr_unit(GyrUnit::Rps) // rad/s → direct uom use
.gyr_range(GyrRange::Dps250) // ±250 °/s — max resolution
.gyr_dlp(GyrDlp::Hz51) // 51 Hz DLPF — 5.6 ms delay
.gyr_odr(10_u8) // 1125 / (1+10) ≈ 102 Hz
.acc_range(AccRange::Gs2) // ±2 g — max resolution
.acc_unit(AccUnit::Mpss) // m/s² → direct uom use
.acc_dlp(AccDlp::Hz50) // 50 Hz — closest to gyro DLPF
.acc_odr(10_u16) // ≈ 102 Hz — match gyro
.initialize_6dof()
.await
.expect("ICM20948 init failed");
// Compute static bias
let mut ticker = Ticker::every(IMU_READ_TICK_RATE);
let mut sum: ImuData<f32> = Default::default();
for _ in 0..IMU_CALC_STATIC_BIAS_SAMPLES {
ticker.next().await;
let data = imu.read_6dof().await.expect("ICM20948 read failed");
sum.acc[0] += data.acc[0];
sum.acc[1] += data.acc[1];
sum.acc[2] += data.acc[2];
sum.gyro[0] += data.gyr[0];
sum.gyro[1] += data.gyr[1];
sum.gyro[2] += data.gyr[2];
sum.temp += data.tmp;
}
let n = IMU_CALC_STATIC_BIAS_SAMPLES as f32;
IMU_STATIC_BIAS.init(ImuData {
acc: [sum.acc[0] / n, sum.acc[1] / n, sum.acc[2] / n],
gyro: [sum.gyro[0] / n, sum.gyro[1] / n, sum.gyro[2] / n],
temp: sum.temp / n,
});
loop {
let data_read = imu.read_6dof().await.expect("ICM20948 read failed");
let data = ImuData {
acc: data_read.acc,
gyro: data_read.gyr,
temp: data_read.tmp,
};
info!(
"Accel -> X: {}, Y: {}, Z: {}",
data.acc[0], data.acc[1], data.acc[2]
);
info!(
"Gyro -> X: {}, Y: {}, Z: {}",
data.gyro[0], data.gyro[1], data.gyro[2]
);
info!("Tmp -> {}", data.temp);
signal.signal(data);
Timer::after(Duration::from_millis(500)).await;
}
}
pub fn imu_get_static_bias() -> ImuData<f32> {
if let Some(static_bias) = IMU_STATIC_BIAS.try_get() {
static_bias
} else {
Default::default()
}
}
problem static_cell has no get() function.