Skip to main content

edrv_mpu6050/
lib.rs

1//! Driver for MPU6050.
2
3#![cfg_attr(not(test), no_std)]
4
5use embedded_hal_async::delay::DelayNs;
6
7pub mod blocking;
8
9pub const PRIMARY_ADDRESS: u8 = 0x68;
10pub const SECONDARY_ADDRESS: u8 = 0x69;
11
12/// `PWR_MGMT_1` value that leaves sleep and selects the X-gyro PLL clock.
13///
14/// Register map RM-MPU-6000A-00, section 4.30: `CLKSEL = 0` is the internal
15/// 8 MHz oscillator and `CLKSEL = 1` is "PLL with X axis gyroscope reference",
16/// and "it is highly recommended that the device be configured to use one of
17/// the gyroscopes (or an external clock source) as the clock reference for
18/// improved stability". The product specification (section 6.6) quantifies that:
19/// the internal oscillator has -5%/+5% initial tolerance and -15%/+10% over
20/// temperature, against +/-1% for the gyro PLL. That error propagates straight
21/// into the sample rate, so `0x00` is not merely a preference.
22///
23/// Bit 6 (SLEEP) is 0, so this also takes the part out of sleep.
24pub const PWR_MGMT_1_CLKSEL_PLL_X_GYRO: u8 = 0x01;
25
26/// How long `init` waits after wake-up, in milliseconds.
27///
28/// The product specification (section 6.1) lists the gyroscope start-up time as
29/// "ZRO Settling (from power-on) to +/-1 dps of Final" = 30 ms typical, and
30/// section 7.15 notes that the MEMS oscillators have to stabilise before they
31/// are selected as the clock source. The sensor registers read as zero until the
32/// first conversion has completed, so without this wait a caller's first read
33/// returns zeros.
34///
35/// Note the datasheet gives this as a **typical** value and for power-on rather
36/// than wake-from-sleep; 30 ms is the closest documented figure and is used as
37/// a floor, not a guarantee.
38pub const WAKE_UP_DELAY_MS: u32 = 30;
39
40pub mod regs {
41    pub const XG_OFFS_TC: u8 = 0x00; //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
42    pub const YG_OFFS_TC: u8 = 0x01; //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
43    pub const ZG_OFFS_TC: u8 = 0x02; //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
44    pub const X_FINE_GAIN: u8 = 0x03; //[7:0] X_FINE_GAIN
45    pub const Y_FINE_GAIN: u8 = 0x04; //[7:0] Y_FINE_GAIN
46    pub const Z_FINE_GAIN: u8 = 0x05; //[7:0] Z_FINE_GAIN
47    pub const XA_OFFS_H: u8 = 0x06; //[15:0] XA_OFFS
48    pub const XA_OFFS_L_TC: u8 = 0x07;
49    pub const YA_OFFS_H: u8 = 0x08; //[15:0] YA_OFFS
50    pub const YA_OFFS_L_TC: u8 = 0x09;
51    pub const ZA_OFFS_H: u8 = 0x0A; //[15:0] ZA_OFFS
52    pub const ZA_OFFS_L_TC: u8 = 0x0B;
53    pub const XG_OFFS_USRH: u8 = 0x13; //[15:0] XG_OFFS_USR
54    pub const XG_OFFS_USRL: u8 = 0x14;
55    pub const YG_OFFS_USRH: u8 = 0x15; //[15:0] YG_OFFS_USR
56    pub const YG_OFFS_USRL: u8 = 0x16;
57    pub const ZG_OFFS_USRH: u8 = 0x17; //[15:0] ZG_OFFS_USR
58    pub const ZG_OFFS_USRL: u8 = 0x18;
59    pub const SMPLRT_DIV: u8 = 0x19;
60    pub const CONFIG: u8 = 0x1A;
61    pub const GYRO_CONFIG: u8 = 0x1B;
62    pub const ACCEL_CONFIG: u8 = 0x1C;
63    pub const FF_THR: u8 = 0x1D;
64    pub const FF_DUR: u8 = 0x1E;
65    pub const MOT_THR: u8 = 0x1F;
66    pub const MOT_DUR: u8 = 0x20;
67    pub const ZRMOT_THR: u8 = 0x21;
68    pub const ZRMOT_DUR: u8 = 0x22;
69    pub const FIFO_EN: u8 = 0x23;
70    pub const I2C_MST_CTRL: u8 = 0x24;
71    pub const I2C_SLV0_ADDR: u8 = 0x25;
72    pub const I2C_SLV0_REG: u8 = 0x26;
73    pub const I2C_SLV0_CTRL: u8 = 0x27;
74    pub const I2C_SLV1_ADDR: u8 = 0x28;
75    pub const I2C_SLV1_REG: u8 = 0x29;
76    pub const I2C_SLV1_CTRL: u8 = 0x2A;
77    pub const I2C_SLV2_ADDR: u8 = 0x2B;
78    pub const I2C_SLV2_REG: u8 = 0x2C;
79    pub const I2C_SLV2_CTRL: u8 = 0x2D;
80    pub const I2C_SLV3_ADDR: u8 = 0x2E;
81    pub const I2C_SLV3_REG: u8 = 0x2F;
82    pub const I2C_SLV3_CTRL: u8 = 0x30;
83    pub const I2C_SLV4_ADDR: u8 = 0x31;
84    pub const I2C_SLV4_REG: u8 = 0x32;
85    pub const I2C_SLV4_DO: u8 = 0x33;
86    pub const I2C_SLV4_CTRL: u8 = 0x34;
87    pub const I2C_SLV4_DI: u8 = 0x35;
88    pub const I2C_MST_STATUS: u8 = 0x36;
89    pub const INT_PIN_CFG: u8 = 0x37;
90    pub const INT_ENABLE: u8 = 0x38;
91    pub const DMP_INT_STATUS: u8 = 0x39;
92    pub const INT_STATUS: u8 = 0x3A;
93    pub const ACCEL_XOUT_H: u8 = 0x3B;
94    pub const ACCEL_XOUT_L: u8 = 0x3C;
95    pub const ACCEL_YOUT_H: u8 = 0x3D;
96    pub const ACCEL_YOUT_L: u8 = 0x3E;
97    pub const ACCEL_ZOUT_H: u8 = 0x3F;
98    pub const ACCEL_ZOUT_L: u8 = 0x40;
99    pub const TEMP_OUT_H: u8 = 0x41;
100    pub const TEMP_OUT_L: u8 = 0x42;
101    pub const GYRO_XOUT_H: u8 = 0x43;
102    pub const GYRO_XOUT_L: u8 = 0x44;
103    pub const GYRO_YOUT_H: u8 = 0x45;
104    pub const GYRO_YOUT_L: u8 = 0x46;
105    pub const GYRO_ZOUT_H: u8 = 0x47;
106    pub const GYRO_ZOUT_L: u8 = 0x48;
107    pub const EXT_SENS_DATA_00: u8 = 0x49;
108    pub const EXT_SENS_DATA_01: u8 = 0x4A;
109    pub const EXT_SENS_DATA_02: u8 = 0x4B;
110    pub const EXT_SENS_DATA_03: u8 = 0x4C;
111    pub const EXT_SENS_DATA_04: u8 = 0x4D;
112    pub const EXT_SENS_DATA_05: u8 = 0x4E;
113    pub const EXT_SENS_DATA_06: u8 = 0x4F;
114    pub const EXT_SENS_DATA_07: u8 = 0x50;
115    pub const EXT_SENS_DATA_08: u8 = 0x51;
116    pub const EXT_SENS_DATA_09: u8 = 0x52;
117    pub const EXT_SENS_DATA_10: u8 = 0x53;
118    pub const EXT_SENS_DATA_11: u8 = 0x54;
119    pub const EXT_SENS_DATA_12: u8 = 0x55;
120    pub const EXT_SENS_DATA_13: u8 = 0x56;
121    pub const EXT_SENS_DATA_14: u8 = 0x57;
122    pub const EXT_SENS_DATA_15: u8 = 0x58;
123    pub const EXT_SENS_DATA_16: u8 = 0x59;
124    pub const EXT_SENS_DATA_17: u8 = 0x5A;
125    pub const EXT_SENS_DATA_18: u8 = 0x5B;
126    pub const EXT_SENS_DATA_19: u8 = 0x5C;
127    pub const EXT_SENS_DATA_20: u8 = 0x5D;
128    pub const EXT_SENS_DATA_21: u8 = 0x5E;
129    pub const EXT_SENS_DATA_22: u8 = 0x5F;
130    pub const EXT_SENS_DATA_23: u8 = 0x60;
131    pub const MOT_DETECT_STATUS: u8 = 0x61;
132    pub const I2C_SLV0_DO: u8 = 0x63;
133    pub const I2C_SLV1_DO: u8 = 0x64;
134    pub const I2C_SLV2_DO: u8 = 0x65;
135    pub const I2C_SLV3_DO: u8 = 0x66;
136    pub const I2C_MST_DELAY_CTRL: u8 = 0x67;
137    pub const SIGNAL_PATH_RESET: u8 = 0x68;
138    pub const MOT_DETECT_CTRL: u8 = 0x69;
139    pub const USER_CTRL: u8 = 0x6A;
140    pub const PWR_MGMT_1: u8 = 0x6B;
141    pub const PWR_MGMT_2: u8 = 0x6C;
142    pub const BANK_SEL: u8 = 0x6D;
143    pub const MEM_START_ADDR: u8 = 0x6E;
144    pub const MEM_R_W: u8 = 0x6F;
145    pub const DMP_CFG_1: u8 = 0x70;
146    pub const DMP_CFG_2: u8 = 0x71;
147    pub const FIFO_COUNTH: u8 = 0x72;
148    pub const FIFO_COUNTL: u8 = 0x73;
149    pub const FIFO_R_W: u8 = 0x74;
150    pub const WHO_AM_I: u8 = 0x75;
151}
152
153pub mod consts {
154    pub const DEV_ID_MPU6050: u8 = 0x68;
155    pub const DEV_ID_MPU6500: u8 = 0x70;
156    pub const DEV_ID_MPU9250: u8 = 0x71;
157    pub const DEV_ID_MPU9255: u8 = 0x73;
158}
159
160#[derive(Debug)]
161pub enum Error<E> {
162    Bus(E),
163    InvalidDevice,
164}
165
166impl<E> From<E> for Error<E> {
167    fn from(e: E) -> Self {
168        Error::Bus(e)
169    }
170}
171
172/// DLPF_CFG
173/// Digital low pass filter configuration
174#[derive(Debug, Clone, Copy, Default)]
175pub enum DlpfCfg {
176    /// 260Hz,
177    Hz260 = 0,
178    /// 184Hz
179    #[default]
180    Hz184 = 1,
181    /// 94Hz
182    Hz94 = 2,
183    /// 44Hz
184    Hz44 = 3,
185    /// 21Hz
186    Hz21 = 4,
187    /// 10Hz
188    Hz10 = 5,
189    /// 5Hz
190    Hz5 = 6,
191}
192
193/// FS_SEL
194#[derive(Debug, Clone, Copy, Default)]
195pub enum GyroRange {
196    /// ±250 degrees per second
197    Deg250 = 0,
198    /// ±500 degrees per second
199    Deg500 = 1,
200    /// ±1000 degrees per second
201    #[default]
202    Deg1000 = 2,
203    /// ±2000 degrees per second
204    Deg2000 = 3,
205}
206
207impl GyroRange {
208    #[inline]
209    fn lsb_sensitivity(&self) -> f32 {
210        match *self {
211            GyroRange::Deg250 => 131.0,
212            GyroRange::Deg500 => 65.5,
213            GyroRange::Deg1000 => 32.8,
214            GyroRange::Deg2000 => 16.4,
215        }
216    }
217
218    /// `FS_SEL` field value for `GYRO_CONFIG`.
219    ///
220    /// `FS_SEL` lives in bits 4:3, so the discriminant has to be shifted.
221    /// Writing it unshifted leaves the chip on its default range while the
222    /// conversion maths assumes the requested one, silently scaling every gyro
223    /// reading (Deg1000 was 4x too large).
224    #[inline]
225    pub const fn config_bits(self) -> u8 {
226        (self as u8) << 3
227    }
228}
229
230/// AFS_SEL
231/// Full scale range for the accelerometer
232#[derive(Debug, Clone, Copy, Default)]
233pub enum AccelRange {
234    /// ±2g
235    #[default]
236    G2 = 0,
237    /// ±4g
238    G4 = 1,
239    /// ±8g
240    G8 = 2,
241    /// ±16g
242    G16 = 3,
243}
244
245impl AccelRange {
246    #[inline]
247    fn lsb_sensitivity(&self) -> f32 {
248        match *self {
249            AccelRange::G2 => 16384.0,
250            AccelRange::G4 => 8192.0,
251            AccelRange::G8 => 4096.0,
252            AccelRange::G16 => 2048.0,
253        }
254    }
255
256    /// `AFS_SEL` field value for `ACCEL_CONFIG`, which also lives in bits 4:3.
257    #[inline]
258    pub const fn config_bits(self) -> u8 {
259        (self as u8) << 3
260    }
261}
262
263#[derive(Debug, Default)]
264pub struct Config {
265    pub lpf: DlpfCfg,
266    pub gyro_range: GyroRange,
267    pub accel_range: AccelRange,
268}
269
270pub struct MPU6050<I2C: embedded_hal_async::i2c::I2c> {
271    addr: u8,
272    i2c: I2C,
273    gyro_range: GyroRange,
274    accel_range: AccelRange,
275}
276
277impl<I2C: embedded_hal_async::i2c::I2c> MPU6050<I2C> {
278    pub fn new(i2c: I2C, addr: u8) -> Self {
279        Self {
280            addr,
281            i2c,
282            gyro_range: GyroRange::Deg1000,
283            accel_range: AccelRange::G2,
284        }
285    }
286
287    pub fn new_primary(i2c: I2C) -> Self {
288        Self::new(i2c, PRIMARY_ADDRESS)
289    }
290
291    /// Validate the part, wake it and wait for the gyro to start up.
292    ///
293    /// `PWR_MGMT_1` is written as [`PWR_MGMT_1_CLKSEL_PLL_X_GYRO`] (leave sleep,
294    /// PLL with X-gyro reference) rather than `0x00`, which would leave the part
295    /// on its internal 8 MHz oscillator. `init` then waits
296    /// [`WAKE_UP_DELAY_MS`] before returning, because the sensor registers read
297    /// zero until the first conversion completes.
298    pub async fn init(&mut self, config: Config, mut delay: impl DelayNs) -> Result<(), Error<I2C::Error>> {
299        let who_am_i = self.read_reg(regs::WHO_AM_I).await?;
300        if who_am_i != consts::DEV_ID_MPU6050
301            && who_am_i != consts::DEV_ID_MPU6500
302            && who_am_i != consts::DEV_ID_MPU9250
303            && who_am_i != consts::DEV_ID_MPU9255
304        {
305            return Err(Error::InvalidDevice);
306        }
307
308        // Exit sleep mode and select the X-gyro PLL as the clock source.
309        self.write_reg(regs::PWR_MGMT_1, PWR_MGMT_1_CLKSEL_PLL_X_GYRO).await?;
310
311        // LPF
312        self.write_reg(regs::CONFIG, config.lpf as u8).await?;
313
314        // gyro ADC scale
315        self.write_reg(regs::GYRO_CONFIG, config.gyro_range.config_bits())
316            .await?;
317
318        // accel ADC scale
319        self.write_reg(regs::ACCEL_CONFIG, config.accel_range.config_bits())
320            .await?;
321
322        self.gyro_range = config.gyro_range;
323        self.accel_range = config.accel_range;
324
325        // The sensor registers hold zero until the gyro has started up.
326        delay.delay_ms(WAKE_UP_DELAY_MS).await;
327
328        Ok(())
329    }
330
331    pub async fn read_raw_accel(&mut self) -> Result<(i16, i16, i16), Error<I2C::Error>> {
332        let mut buf = [0u8; 6];
333        self.read_regs(regs::ACCEL_XOUT_H, &mut buf).await?;
334
335        let x = i16::from_be_bytes([buf[0], buf[1]]);
336        let y = i16::from_be_bytes([buf[2], buf[3]]);
337        let z = i16::from_be_bytes([buf[4], buf[5]]);
338
339        Ok((x, y, z))
340    }
341
342    pub async fn read_raw_gyro(&mut self) -> Result<(i16, i16, i16), Error<I2C::Error>> {
343        let mut buf = [0u8; 6];
344        self.read_regs(regs::GYRO_XOUT_H, &mut buf).await?;
345
346        let x = i16::from_be_bytes([buf[0], buf[1]]);
347        let y = i16::from_be_bytes([buf[2], buf[3]]);
348        let z = i16::from_be_bytes([buf[4], buf[5]]);
349
350        Ok((x, y, z))
351    }
352
353    /// Read accelerometer data in g
354    pub async fn read_accel(&mut self) -> Result<(f32, f32, f32), Error<I2C::Error>> {
355        let (x, y, z) = self.read_raw_accel().await?;
356        let lsb_sensitivity = self.accel_range.lsb_sensitivity();
357        Ok((
358            x as f32 / lsb_sensitivity,
359            y as f32 / lsb_sensitivity,
360            z as f32 / lsb_sensitivity,
361        ))
362    }
363
364    /// Read gyroscope data in degrees per second
365    pub async fn read_gyro(&mut self) -> Result<(f32, f32, f32), Error<I2C::Error>> {
366        let (x, y, z) = self.read_raw_gyro().await?;
367        let lsb_sensitivity = self.gyro_range.lsb_sensitivity();
368        Ok((
369            x as f32 / lsb_sensitivity,
370            y as f32 / lsb_sensitivity,
371            z as f32 / lsb_sensitivity,
372        ))
373    }
374
375    /// Read temperature in degrees Celsius
376    pub async fn read_temperature(&mut self) -> Result<f32, Error<I2C::Error>> {
377        let mut buf = [0u8; 2];
378        self.read_regs(regs::TEMP_OUT_H, &mut buf).await?;
379
380        let temp = i16::from_be_bytes([buf[0], buf[1]]);
381        Ok((temp as f32) / 340.0 + 36.53)
382    }
383
384    pub async fn read_reg(&mut self, reg: u8) -> Result<u8, Error<I2C::Error>> {
385        let mut buf = [0u8; 1];
386        self.i2c.write_read(self.addr, &[reg], &mut buf).await?;
387        Ok(buf[0])
388    }
389
390    pub async fn read_regs(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), Error<I2C::Error>> {
391        self.i2c.write_read(self.addr, &[reg], buf).await?;
392        Ok(())
393    }
394
395    pub async fn write_reg(&mut self, reg: u8, value: u8) -> Result<(), Error<I2C::Error>> {
396        self.i2c.write(self.addr, &[reg, value]).await?;
397        Ok(())
398    }
399}
400
401#[cfg(test)]
402mod tests {
403    use super::*;
404
405    #[test]
406    fn gyro_range_lands_in_fs_sel() {
407        // Regression test: these used to be written unshifted, so every range
408        // except Deg250 silently left the chip on +/-250 deg/s.
409        assert_eq!(GyroRange::Deg250.config_bits(), 0b0000_0000);
410        assert_eq!(GyroRange::Deg500.config_bits(), 0b0000_1000);
411        assert_eq!(GyroRange::Deg1000.config_bits(), 0b0001_0000);
412        assert_eq!(GyroRange::Deg2000.config_bits(), 0b0001_1000);
413
414        for range in [
415            GyroRange::Deg250,
416            GyroRange::Deg500,
417            GyroRange::Deg1000,
418            GyroRange::Deg2000,
419        ] {
420            let fs_sel = (range.config_bits() >> 3) & 0b11;
421            assert_eq!(fs_sel, range as u8, "FS_SEL must encode the requested range");
422        }
423    }
424
425    #[test]
426    fn accel_range_lands_in_afs_sel() {
427        assert_eq!(AccelRange::G2.config_bits(), 0b0000_0000);
428        assert_eq!(AccelRange::G4.config_bits(), 0b0000_1000);
429        assert_eq!(AccelRange::G8.config_bits(), 0b0001_0000);
430        assert_eq!(AccelRange::G16.config_bits(), 0b0001_1000);
431
432        for range in [AccelRange::G2, AccelRange::G4, AccelRange::G8, AccelRange::G16] {
433            let afs_sel = (range.config_bits() >> 3) & 0b11;
434            assert_eq!(afs_sel, range as u8, "AFS_SEL must encode the requested range");
435        }
436    }
437}