143 lines
4.2 KiB
Rust
143 lines
4.2 KiB
Rust
// src/mpu/task.rs
|
|
//! MPU6050 sampling task.
|
|
//!
|
|
//! This task:
|
|
//! 1. Initializes the MPU6050 sensor
|
|
//! 2. Continuously reads sensor data at a fixed rate
|
|
//! 3. Publishes readings to the IMU channel
|
|
|
|
use embassy_time::{Duration, Instant, Timer};
|
|
use log::{error, info, warn};
|
|
|
|
use crate::bus::I2cDevice;
|
|
use crate::contracts::ImuReading;
|
|
use crate::mpu::api::sender;
|
|
use crate::mpu::driver::Mpu6050;
|
|
|
|
/// Sampling interval in milliseconds.
|
|
/// 50ms = 20Hz
|
|
const SAMPLE_INTERVAL_MS: u64 = 50;
|
|
|
|
/// MPU6050 I2C address (0x68 with AD0 low, 0x69 with AD0 high)
|
|
const MPU_ADDR: u8 = 0x68;
|
|
|
|
/// The MPU6050 sampling task.
|
|
///
|
|
/// # Arguments
|
|
/// * `i2c` - An I2C device handle from the shared bus
|
|
///
|
|
/// # Panics
|
|
/// Does not panic; logs errors and retries on failure.
|
|
#[embassy_executor::task]
|
|
pub async fn mpu_task(i2c: I2cDevice) {
|
|
info!("MPU task starting...");
|
|
|
|
let mut mpu = Mpu6050::new(i2c, MPU_ADDR);
|
|
|
|
// Initialize with retries
|
|
let mut init_attempts = 0;
|
|
loop {
|
|
init_attempts += 1;
|
|
|
|
// Verify device is present
|
|
match mpu.verify() {
|
|
Ok(true) => {
|
|
info!("MPU6050 detected at 0x{:02X}", MPU_ADDR);
|
|
}
|
|
Ok(false) => {
|
|
warn!(
|
|
"Device at 0x{:02X} is not MPU6050 (attempt {})",
|
|
MPU_ADDR, init_attempts
|
|
);
|
|
Timer::after(Duration::from_secs(2)).await;
|
|
continue;
|
|
}
|
|
Err(_) => {
|
|
warn!(
|
|
"I2C error verifying MPU6050 (attempt {})",
|
|
init_attempts
|
|
);
|
|
Timer::after(Duration::from_secs(2)).await;
|
|
continue;
|
|
}
|
|
}
|
|
|
|
// Initialize sensor
|
|
match mpu.init() {
|
|
Ok(()) => {
|
|
info!("MPU6050 initialized successfully");
|
|
break;
|
|
}
|
|
Err(_) => {
|
|
error!("MPU6050 init failed (attempt {})", init_attempts);
|
|
Timer::after(Duration::from_secs(2)).await;
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Allow sensor to stabilize after wake-up
|
|
Timer::after(Duration::from_millis(100)).await;
|
|
info!(
|
|
"MPU task entering sampling loop ({}ms interval)",
|
|
SAMPLE_INTERVAL_MS
|
|
);
|
|
|
|
let tx = sender();
|
|
let mut consecutive_errors = 0u32;
|
|
let mut sent_count: u32 = 0;
|
|
|
|
loop {
|
|
let start = Instant::now();
|
|
|
|
match mpu.read() {
|
|
Ok((accel_g, gyro_dps, temp_c)) => {
|
|
consecutive_errors = 0;
|
|
|
|
let reading = ImuReading {
|
|
accel_g,
|
|
gyro_dps,
|
|
temp_c,
|
|
timestamp_ms: start.as_millis(),
|
|
};
|
|
|
|
sent_count = sent_count.wrapping_add(1);
|
|
if tx.try_send(reading).is_ok() {
|
|
if sent_count % 20 == 0 {
|
|
info!(
|
|
"IMU send#{} ax:{:.2} ay:{:.2} az:{:.2} t:{:.1}",
|
|
sent_count, accel_g[0], accel_g[1], accel_g[2], temp_c
|
|
);
|
|
}
|
|
} else if sent_count % 20 == 0 {
|
|
info!("IMU drop: channel full ({} sent)", sent_count);
|
|
}
|
|
}
|
|
|
|
Err(_) => {
|
|
consecutive_errors += 1;
|
|
if consecutive_errors == 1 || consecutive_errors % 10 == 0 {
|
|
warn!("MPU read error (consecutive: {})", consecutive_errors);
|
|
}
|
|
|
|
// If too many errors, try to reinitialize
|
|
if consecutive_errors >= 50 {
|
|
error!("Too many MPU errors, attempting reinit...");
|
|
if mpu.init().is_ok() {
|
|
info!("MPU reinit successful");
|
|
consecutive_errors = 0;
|
|
Timer::after(Duration::from_millis(100)).await;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Maintain a steady period
|
|
let elapsed = start.elapsed();
|
|
let target = Duration::from_millis(SAMPLE_INTERVAL_MS);
|
|
if elapsed < target {
|
|
Timer::after(target - elapsed).await;
|
|
}
|
|
}
|
|
}
|