242 lines
7.1 KiB
Rust
242 lines
7.1 KiB
Rust
// src/mpu/driver.rs
|
|
//! MPU6050 driver using raw register I/O.
|
|
|
|
use embedded_hal::i2c::I2c;
|
|
|
|
/// MPU6050 register addresses
|
|
mod reg {
|
|
pub const PWR_MGMT_1: u8 = 0x6B;
|
|
pub const CONFIG: u8 = 0x1A;
|
|
pub const GYRO_CONFIG: u8 = 0x1B;
|
|
pub const ACCEL_CONFIG: u8 = 0x1C;
|
|
pub const ACCEL_XOUT_H: u8 = 0x3B; // Start of 14-byte burst read
|
|
pub const WHO_AM_I: u8 = 0x75;
|
|
}
|
|
|
|
/// Expected WHO_AM_I value for MPU6050
|
|
const MPU6050_WHO_AM_I: u8 = 0x68;
|
|
|
|
/// MPU6050 accelerometer full-scale range
|
|
#[derive(Clone, Copy, Debug, Default)]
|
|
pub enum AccelRange {
|
|
#[default]
|
|
G2 = 0, // ±2g -> 16384 LSB/g
|
|
G4 = 1, // ±4g -> 8192 LSB/g
|
|
G8 = 2, // ±8g -> 4096 LSB/g
|
|
G16 = 3, // ±16g -> 2048 LSB/g
|
|
}
|
|
|
|
impl AccelRange {
|
|
/// LSB per g for this range
|
|
pub fn sensitivity(self) -> f32 {
|
|
match self {
|
|
AccelRange::G2 => 16384.0,
|
|
AccelRange::G4 => 8192.0,
|
|
AccelRange::G8 => 4096.0,
|
|
AccelRange::G16 => 2048.0,
|
|
}
|
|
}
|
|
}
|
|
|
|
/// MPU6050 gyroscope full-scale range
|
|
#[derive(Clone, Copy, Debug, Default)]
|
|
pub enum GyroRange {
|
|
#[default]
|
|
Dps250 = 0, // ±250°/s -> 131 LSB/°/s
|
|
Dps500 = 1, // ±500°/s -> 65.5 LSB/°/s
|
|
Dps1000 = 2, // ±1000°/s -> 32.8 LSB/°/s
|
|
Dps2000 = 3, // ±2000°/s -> 16.4 LSB/°/s
|
|
}
|
|
|
|
impl GyroRange {
|
|
/// LSB per degree/second for this range
|
|
pub fn sensitivity(self) -> f32 {
|
|
match self {
|
|
GyroRange::Dps250 => 131.0,
|
|
GyroRange::Dps500 => 65.5,
|
|
GyroRange::Dps1000 => 32.8,
|
|
GyroRange::Dps2000 => 16.4,
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Raw sensor data from a single read
|
|
#[derive(Clone, Copy, Debug, Default)]
|
|
pub struct RawReading {
|
|
pub accel: [i16; 3], // X, Y, Z
|
|
pub gyro: [i16; 3], // X, Y, Z
|
|
pub temp: i16,
|
|
}
|
|
|
|
/// MPU6050 driver
|
|
pub struct Mpu6050<I> {
|
|
i2c: I,
|
|
addr: u8,
|
|
accel_range: AccelRange,
|
|
gyro_range: GyroRange,
|
|
}
|
|
|
|
impl<I> Mpu6050<I> {
|
|
/// Create a new MPU6050 driver.
|
|
///
|
|
/// Default I2C address is 0x68. Use 0x69 if AD0 pin is high.
|
|
pub fn new(i2c: I, addr: u8) -> Self {
|
|
Self {
|
|
i2c,
|
|
addr,
|
|
accel_range: AccelRange::default(),
|
|
gyro_range: GyroRange::default(),
|
|
}
|
|
}
|
|
|
|
/// Create with default address 0x68
|
|
pub fn new_default(i2c: I) -> Self {
|
|
Self::new(i2c, 0x68)
|
|
}
|
|
|
|
/// Release the I2C peripheral
|
|
pub fn release(self) -> I {
|
|
self.i2c
|
|
}
|
|
|
|
/// Get current accelerometer range
|
|
pub fn accel_range(&self) -> AccelRange {
|
|
self.accel_range
|
|
}
|
|
|
|
/// Get current gyroscope range
|
|
pub fn gyro_range(&self) -> GyroRange {
|
|
self.gyro_range
|
|
}
|
|
}
|
|
|
|
impl<I, E> Mpu6050<I>
|
|
where
|
|
I: I2c<Error = E>,
|
|
{
|
|
/// Initialize the sensor with default configuration.
|
|
///
|
|
/// - Wakes up the device (exits sleep mode)
|
|
/// - Sets DLPF to ~44Hz bandwidth
|
|
/// - Sets accel range to ±2g
|
|
/// - Sets gyro range to ±250°/s
|
|
///
|
|
/// Call this once after creating the driver.
|
|
/// Consider adding a ~50ms delay after init before first read.
|
|
pub fn init(&mut self) -> Result<(), E> {
|
|
// Wake up (clear sleep bit in PWR_MGMT_1)
|
|
self.write_reg(reg::PWR_MGMT_1, 0x00)?;
|
|
|
|
// Set DLPF (Digital Low Pass Filter) to ~44Hz
|
|
// CONFIG register, bits 2:0 = DLPF_CFG
|
|
self.write_reg(reg::CONFIG, 0x03)?;
|
|
|
|
// Set accelerometer range (default ±2g)
|
|
self.write_reg(reg::ACCEL_CONFIG, (self.accel_range as u8) << 3)?;
|
|
|
|
// Set gyroscope range (default ±250°/s)
|
|
self.write_reg(reg::GYRO_CONFIG, (self.gyro_range as u8) << 3)?;
|
|
|
|
Ok(())
|
|
}
|
|
|
|
/// Verify the device is responding and is an MPU6050.
|
|
///
|
|
/// Returns true if WHO_AM_I register returns expected value.
|
|
pub fn verify(&mut self) -> Result<bool, E> {
|
|
let who = self.read_reg(reg::WHO_AM_I)?;
|
|
Ok(who == MPU6050_WHO_AM_I)
|
|
}
|
|
|
|
/// Set accelerometer full-scale range.
|
|
///
|
|
/// Takes effect immediately.
|
|
pub fn set_accel_range(&mut self, range: AccelRange) -> Result<(), E> {
|
|
self.accel_range = range;
|
|
self.write_reg(reg::ACCEL_CONFIG, (range as u8) << 3)
|
|
}
|
|
|
|
/// Set gyroscope full-scale range.
|
|
///
|
|
/// Takes effect immediately.
|
|
pub fn set_gyro_range(&mut self, range: GyroRange) -> Result<(), E> {
|
|
self.gyro_range = range;
|
|
self.write_reg(reg::GYRO_CONFIG, (range as u8) << 3)
|
|
}
|
|
|
|
/// Read all sensor data in a single burst.
|
|
///
|
|
/// This reads 14 bytes starting at ACCEL_XOUT_H:
|
|
/// - Accelerometer X, Y, Z (6 bytes)
|
|
/// - Temperature (2 bytes)
|
|
/// - Gyroscope X, Y, Z (6 bytes)
|
|
///
|
|
/// Returns raw values; use `convert_reading` for physical units.
|
|
pub fn read_raw(&mut self) -> Result<RawReading, E> {
|
|
let mut buf = [0u8; 14];
|
|
self.i2c.write_read(self.addr, &[reg::ACCEL_XOUT_H], &mut buf)?;
|
|
|
|
Ok(RawReading {
|
|
accel: [
|
|
i16::from_be_bytes([buf[0], buf[1]]),
|
|
i16::from_be_bytes([buf[2], buf[3]]),
|
|
i16::from_be_bytes([buf[4], buf[5]]),
|
|
],
|
|
temp: i16::from_be_bytes([buf[6], buf[7]]),
|
|
gyro: [
|
|
i16::from_be_bytes([buf[8], buf[9]]),
|
|
i16::from_be_bytes([buf[10], buf[11]]),
|
|
i16::from_be_bytes([buf[12], buf[13]]),
|
|
],
|
|
})
|
|
}
|
|
|
|
/// Convert raw reading to physical units.
|
|
///
|
|
/// Returns (accel_g, gyro_dps, temp_c)
|
|
pub fn convert_reading(&self, raw: &RawReading) -> ([f32; 3], [f32; 3], f32) {
|
|
let accel_sens = self.accel_range.sensitivity();
|
|
let gyro_sens = self.gyro_range.sensitivity();
|
|
|
|
let accel_g = [
|
|
raw.accel[0] as f32 / accel_sens,
|
|
raw.accel[1] as f32 / accel_sens,
|
|
raw.accel[2] as f32 / accel_sens,
|
|
];
|
|
|
|
let gyro_dps = [
|
|
raw.gyro[0] as f32 / gyro_sens,
|
|
raw.gyro[1] as f32 / gyro_sens,
|
|
raw.gyro[2] as f32 / gyro_sens,
|
|
];
|
|
|
|
// Temperature formula from datasheet:
|
|
// Temp in °C = (TEMP_OUT / 340.0) + 36.53
|
|
let temp_c = (raw.temp as f32 / 340.0) + 36.53;
|
|
|
|
(accel_g, gyro_dps, temp_c)
|
|
}
|
|
|
|
/// Read and convert in one call.
|
|
///
|
|
/// Convenience method that combines `read_raw` and `convert_reading`.
|
|
pub fn read(&mut self) -> Result<([f32; 3], [f32; 3], f32), E> {
|
|
let raw = self.read_raw()?;
|
|
Ok(self.convert_reading(&raw))
|
|
}
|
|
|
|
// ─────────────────────────────────────────────────────────────────
|
|
// Low-level register access
|
|
// ─────────────────────────────────────────────────────────────────
|
|
|
|
fn write_reg(&mut self, reg: u8, val: u8) -> Result<(), E> {
|
|
self.i2c.write(self.addr, &[reg, val])
|
|
}
|
|
|
|
fn read_reg(&mut self, reg: u8) -> Result<u8, E> {
|
|
let mut buf = [0u8];
|
|
self.i2c.write_read(self.addr, &[reg], &mut buf)?;
|
|
Ok(buf[0])
|
|
}
|
|
}
|