mpu6886/
lib.rs

1//! # mpu6886 sensor driver.
2//!
3//! `embedded_hal` based driver with i2c access to mpu6886
4//!
5//! ### Misc
6//! * [Register sheet](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf),
7//! * [Data sheet](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6500-Datasheet2.pdf)
8//! 
9//! To use this driver you must provide a concrete `embedded_hal` implementation.
10//! This example uses `linux_embedded_hal`.
11//!
12//! **More Examples** can be found [here](https://github.com/juliangaal/mpu6886/tree/master/examples).
13//! ```no_run
14//! use mpu6886::*;
15//! use linux_embedded_hal::{I2cdev, Delay};
16//! use i2cdev::linux::LinuxI2CError;
17//!
18//! fn main() -> Result<(), mpu6886Error<LinuxI2CError>> {
19//!     let i2c = I2cdev::new("/dev/i2c-1")
20//!         .map_err(mpu6886Error::I2c)?;
21//!
22//!     let mut delay = Delay;
23//!     let mut mpu = mpu6886::new(i2c);
24//!
25//!     mpu.init(&mut delay)?;
26//!
27//!     loop {
28//!         // get roll and pitch estimate
29//!         let acc = mpu.get_acc_angles()?;
30//!         println!("r/p: {:?}", acc);
31//!
32//!         // get sensor temp
33//!         let temp = mpu.get_temp()?;
34//!         printlnasd!("temp: {:?}c", temp);
35//!
36//!         // get gyro data, scaled with sensitivity
37//!         let gyro = mpu.get_gyro()?;
38//!         println!("gyro: {:?}", gyro);
39//!
40//!         // get accelerometer data, scaled with sensitivity
41//!         let acc = mpu.get_acc()?;
42//!         println!("acc: {:?}", acc);
43//!     }
44//! }
45//! ```
46
47#![no_std]
48
49mod bits;
50pub mod device;
51
52use crate::device::*;
53use libm::{powf, atan2f, sqrtf};
54use nalgebra::{Vector3, Vector2};
55use embedded_hal::{
56    blocking::delay::DelayMs,
57    blocking::i2c::{Write, WriteRead},
58};
59//use esp_println::println;
60/// PI, f32
61pub const PI: f32 = core::f32::consts::PI;
62
63/// PI / 180, for conversion to radians
64pub const PI_180: f32 = PI / 180.0;
65
66/// All possible errors in this crate
67#[derive(Debug)]
68pub enum Mpu6886Error<E> {
69    /// I2C bus error
70    I2c(E),
71
72    /// Invalid chip ID was read
73    InvalidChipId(u8),
74}
75
76/// Handles all operations on/with mpu6886
77pub struct Mpu6886<I> {
78    i2c: I,
79    slave_addr: u8,
80    acc_sensitivity: f32,
81    gyro_sensitivity: f32,
82}
83
84impl<I, E> Mpu6886<I>
85where
86    I: Write<Error = E> + WriteRead<Error = E>, 
87{
88    /// Side effect free constructor with default sensitivies, no calibration
89    pub fn new(i2c: I) -> Self {
90        Mpu6886 {
91            i2c,
92            slave_addr: DEFAULT_SLAVE_ADDR,
93            acc_sensitivity: ACCEL_SENS.0,
94            gyro_sensitivity: GYRO_SENS.0,
95        }
96    }
97
98    /// custom sensitivity
99    pub fn new_with_sens(i2c: I, arange: AccelRange, grange: GyroRange) -> Self {
100        Mpu6886 {
101            i2c,
102            slave_addr: DEFAULT_SLAVE_ADDR,
103            acc_sensitivity: arange.sensitivity(),
104            gyro_sensitivity: grange.sensitivity(),
105        }
106    }
107
108    /// Same as `new`, but the chip address can be specified (e.g. 0x69, if the A0 pin is pulled up)
109    pub fn new_with_addr(i2c: I, slave_addr: u8) -> Self {
110        Mpu6886 {
111            i2c,
112            slave_addr,
113            acc_sensitivity: ACCEL_SENS.0,
114            gyro_sensitivity: GYRO_SENS.0,
115        }
116    }
117
118    /// Combination of `new_with_sens` and `new_with_addr`
119    pub fn new_with_addr_and_sens(i2c: I, slave_addr: u8, arange: AccelRange, grange: GyroRange) -> Self {
120        Mpu6886 {
121            i2c,
122            slave_addr,
123            acc_sensitivity: arange.sensitivity(),
124            gyro_sensitivity: grange.sensitivity(),
125        }
126    }
127
128    /// Wakes mpu6886 with all sensors enabled (default)
129    fn wake<D: DelayMs<u8>>(&mut self, delay: &mut D) -> Result<(), Mpu6886Error<E>> {
130        // mpu6886 has sleep enabled by default -> set bit 0 to wake
131        // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 (See Register Map )
132        self.write_byte(PWR_MGMT_1::ADDR, 0x01)?;
133        delay.delay_ms(100u8);
134        Ok(())
135    }
136
137    /// From Register map:
138    /// "An  internal  8MHz  oscillator,  gyroscope based  clock,or  external  sources  can  be
139    /// selected  as the MPU-60X0 clock source.
140    /// When the internal 8 MHz oscillator or an external source is chosen as the clock source,
141    /// the MPU-60X0 can operate in low power modes with the gyroscopes disabled. Upon power up,
142    /// the MPU-60X0clock source defaults to the internal oscillator. However, it is highly
143    /// recommended  that  the  device beconfigured  to  use  one  of  the  gyroscopes
144    /// (or  an  external  clocksource) as the clock reference for improved stability.
145    /// The clock source can be selected according to the following table...."
146    pub fn set_clock_source(&mut self, source: CLKSEL) -> Result<(), Mpu6886Error<E>> {
147        Ok(self.write_bits(PWR_MGMT_1::ADDR, PWR_MGMT_1::CLKSEL.bit, PWR_MGMT_1::CLKSEL.length, source as u8)?)
148    }
149
150    /// get current clock source
151    pub fn get_clock_source(&mut self) -> Result<CLKSEL, Mpu6886Error<E>> {
152        let source = self.read_bits(PWR_MGMT_1::ADDR, PWR_MGMT_1::CLKSEL.bit, PWR_MGMT_1::CLKSEL.length)?;
153        Ok(CLKSEL::from(source))
154    }
155
156    /// Init wakes mpu6886 and verifies register addr, e.g. in i2c
157    pub fn init<D: DelayMs<u8>>(&mut self, delay: &mut D) -> Result<(), Mpu6886Error<E>> {
158        self.wake(delay)?;
159        self.verify()?;
160        self.set_accel_range(AccelRange::G2)?;
161        self.set_gyro_range(GyroRange::D250)?;
162        Ok(())
163    }
164
165    /// Verifies device to address 0x68 with WHOAMI.addr() Register
166    fn verify(&mut self) -> Result<(), Mpu6886Error<E>> {
167        let chip_type = self.read_byte(WHOAMI)?;
168        if chip_type != 0x19 {
169            return Err(Mpu6886Error::InvalidChipId(chip_type));
170        }
171        Ok(())
172    }
173
174    /// setup motion detection
175    /// sources:
176    /// * https://github.com/kriswiner/mpu6886/blob/a7e0c8ba61a56c5326b2bcd64bc81ab72ee4616b/mpu6886IMU.ino#L486
177    /// * https://arduino.stackexchange.com/a/48430
178    pub fn setup_motion_detection(&mut self) -> Result<(), Mpu6886Error<E>> {
179        self.write_byte(0x6B, 0x00)?;
180        // optional? self.write_byte(0x68, 0x07)?; // Reset all internal signal paths in the MPU-6050 by writing 0x07 to register 0x68;
181        self.write_byte(INT_PIN_CFG::ADDR, 0x20)?; //write register 0x37 to select how to use the interrupt pin. For an active high, push-pull signal that stays until register (decimal) 58 is read, write 0x20.
182        self.write_byte(ACCEL_CONFIG::ADDR, 0x01)?; //Write register 28 (==0x1C) to set the Digital High Pass Filter, bits 3:0. For example set it to 0x01 for 5Hz. (These 3 bits are grey in the data sheet, but they are used! Leaving them 0 means the filter always outputs 0.)
183        self.write_byte(MOT_THR, 10)?; //Write the desired Motion threshold to register 0x1F (For example, write decimal 20).
184        self.write_byte(MOT_DUR, 40)?; //Set motion detect duration to 1  ms; LSB is 1 ms @ 1 kHz rate
185        self.write_byte(0x69, 0x15)?; //to register 0x69, write the motion detection decrement and a few other settings (for example write 0x15 to set both free-fall and motion decrements to 1 and accelerometer start-up delay to 5ms total by adding 1ms. )
186        self.write_byte(INT_ENABLE::ADDR, 0x40)?; //write register 0x38, bit 6 (0x40), to enable motion detection interrupt.
187        Ok(())
188    }
189
190    /// get whether or not WOM has been detected (INT_STATUS) one of (WOM_X_INT, WOM_Y_INT, WOM_Z_INT)
191    pub fn get_motion_detected(&mut self) -> Result<bool, Mpu6886Error<E>> {
192        let mask = INT_STATUS::WOM_X_INT | INT_STATUS::WOM_Y_INT | INT_STATUS::WOM_Z_INT;
193        Ok(self.read_bit(INT_STATUS::ADDR, mask)? != 0)
194    }
195
196
197    /// Set gyro range, and update sensitivity accordingly
198    pub fn set_gyro_range(&mut self, range: GyroRange) -> Result<(), Mpu6886Error<E>> {
199        self.write_bits(GYRO_CONFIG::ADDR,
200                        GYRO_CONFIG::FS_SEL.bit,
201                        GYRO_CONFIG::FS_SEL.length,
202                        range as u8)?;
203
204        self.gyro_sensitivity = range.sensitivity();
205        Ok(())
206    }
207
208    /// get current gyro range
209    pub fn get_gyro_range(&mut self) -> Result<GyroRange, Mpu6886Error<E>> {
210        let byte = self.read_bits(GYRO_CONFIG::ADDR,
211                                  GYRO_CONFIG::FS_SEL.bit,
212                                  GYRO_CONFIG::FS_SEL.length)?;
213
214        Ok(GyroRange::from(byte))
215    }
216
217    /// set accel range, and update sensitivy accordingly
218    pub fn set_accel_range(&mut self, range: AccelRange) -> Result<(), Mpu6886Error<E>> {
219        self.write_bits(ACCEL_CONFIG::ADDR,
220                        ACCEL_CONFIG::FS_SEL.bit,
221                        ACCEL_CONFIG::FS_SEL.length,
222                        range as u8)?;
223
224        self.acc_sensitivity = range.sensitivity();
225        Ok(())
226    }
227
228    /// get current accel_range
229    pub fn get_accel_range(&mut self) -> Result<AccelRange, Mpu6886Error<E>> {
230        let byte = self.read_bits(ACCEL_CONFIG::ADDR,
231                                  ACCEL_CONFIG::FS_SEL.bit,
232                                  ACCEL_CONFIG::FS_SEL.length)?;
233
234        Ok(AccelRange::from(byte))
235    }
236
237    /// reset device
238    pub fn reset_device<D: DelayMs<u8>>(&mut self, delay: &mut D) -> Result<(), Mpu6886Error<E>> {
239        self.write_bit(PWR_MGMT_1::ADDR, PWR_MGMT_1::DEVICE_RESET, true)?;
240        delay.delay_ms(100u8);
241        // Note: Reset sets sleep to true! Section register map: resets PWR_MGMT to 0x40
242        Ok(())
243    }
244
245    /// enable, disable sleep of sensor
246    pub fn set_sleep_enabled(&mut self, enable: bool) -> Result<(), Mpu6886Error<E>> {
247        Ok(self.write_bit(PWR_MGMT_1::ADDR, PWR_MGMT_1::SLEEP, enable)?)
248    }
249
250    /// get sleep status
251    pub fn get_sleep_enabled(&mut self) -> Result<bool, Mpu6886Error<E>> {
252        Ok(self.read_bit(PWR_MGMT_1::ADDR, PWR_MGMT_1::SLEEP)? != 0)
253    }
254
255    /// enable, disable temperature measurement of sensor
256    /// TEMP_DIS actually saves "disabled status"
257    /// 1 is disabled! -> enable=true : bit=!enable
258    pub fn set_temp_enabled(&mut self, enable: bool) -> Result<(), Mpu6886Error<E>> {
259        Ok(self.write_bit(PWR_MGMT_1::ADDR, PWR_MGMT_1::TEMP_DIS, !enable)?)
260    }
261
262    /// get temperature sensor status
263    /// TEMP_DIS actually saves "disabled status"
264    /// 1 is disabled! -> 1 == 0 : false, 0 == 0 : true
265    pub fn get_temp_enabled(&mut self) -> Result<bool, Mpu6886Error<E>> {
266        Ok(self.read_bit(PWR_MGMT_1::ADDR, PWR_MGMT_1::TEMP_DIS)? == 0)
267    }
268
269    /// set accel x self test
270    pub fn set_accel_x_self_test(&mut self, enable: bool) -> Result<(), Mpu6886Error<E>> {
271        Ok(self.write_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::XA_ST, enable)?)
272    }
273
274    /// get accel x self test
275    pub fn get_accel_x_self_test(&mut self) -> Result<bool, Mpu6886Error<E>> {
276        Ok(self.read_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::XA_ST)? != 0)
277    }
278
279    /// set accel y self test
280    pub fn set_accel_y_self_test(&mut self, enable: bool) -> Result<(), Mpu6886Error<E>> {
281        Ok(self.write_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::YA_ST, enable)?)
282    }
283
284    /// get accel y self test
285    pub fn get_accel_y_self_test(&mut self) -> Result<bool, Mpu6886Error<E>> {
286        Ok(self.read_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::YA_ST)? != 0)
287    }
288
289    /// set accel z self test
290    pub fn set_accel_z_self_test(&mut self, enable: bool) -> Result<(), Mpu6886Error<E>> {
291        Ok(self.write_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::ZA_ST, enable)?)
292    }
293
294    /// get accel z self test
295    pub fn get_accel_z_self_test(&mut self) -> Result<bool, Mpu6886Error<E>> {
296        Ok(self.read_bit(ACCEL_CONFIG::ADDR, ACCEL_CONFIG::ZA_ST)? != 0)
297    }
298
299    /// Roll and pitch estimation from raw accelerometer readings
300    /// NOTE: no yaw! no magnetometer present on mpu6886
301    /// https://www.nxp.com/docs/en/application-note/AN3461.pdf equation 28, 29
302    pub fn get_acc_angles(&mut self) -> Result<Vector2<f32>, Mpu6886Error<E>> {
303        let acc = self.get_acc()?;
304
305        Ok(Vector2::<f32>::new(
306            atan2f(acc.y, sqrtf(powf(acc.x, 2.) + powf(acc.z, 2.))),
307            atan2f(-acc.x, sqrtf(powf(acc.y, 2.) + powf(acc.z, 2.)))
308        ))
309    }
310
311    /// Converts 2 bytes number in 2 compliment
312    /// TODO i16?! whats 0x8000?!
313    fn read_word_2c(&self, byte: &[u8]) -> i32 {
314        let high: i32 = byte[0] as i32;
315        let low: i32 = byte[1] as i32;
316        let mut word: i32 = (high << 8) + low;
317
318        if word >= 0x8000 {
319            word = -((65535 - word) + 1);
320        }
321
322        word
323    }
324
325    /// Reads rotation (gyro/acc) from specified register
326    fn read_rot(&mut self, reg: u8) -> Result<Vector3<f32>, Mpu6886Error<E>> {
327        let mut buf: [u8; 6] = [0; 6];
328        self.read_bytes(reg, &mut buf)?;
329
330        Ok(Vector3::<f32>::new(
331            self.read_word_2c(&buf[0..2]) as f32,
332            self.read_word_2c(&buf[2..4]) as f32,
333            self.read_word_2c(&buf[4..6]) as f32
334        ))
335    }
336
337    /// Accelerometer readings in g
338    pub fn get_acc(&mut self) -> Result<Vector3<f32>, Mpu6886Error<E>> {
339        let mut acc = self.read_rot(ACC_REGX_H)?;
340        acc /= self.acc_sensitivity;
341
342        Ok(acc)
343    }
344
345    /// Gyro readings in rad/s
346    pub fn get_gyro(&mut self) -> Result<Vector3<f32>, Mpu6886Error<E>> {
347        let mut gyro = self.read_rot(GYRO_REGX_H)?;
348
349        gyro *= PI_180 / self.gyro_sensitivity;
350
351        Ok(gyro)
352    }
353
354    /// Sensor Temp in degrees celcius
355    pub fn get_temp(&mut self) -> Result<f32, Mpu6886Error<E>> {
356        let mut buf: [u8; 2] = [0; 2];
357        self.read_bytes(TEMP_OUT_H, &mut buf)?;
358        let raw_temp = self.read_word_2c(&buf[0..2]) as f32;
359
360        // According to revision 4.2
361        Ok((raw_temp / TEMP_SENSITIVITY) + TEMP_OFFSET)
362    }
363
364    /// Writes byte to register
365    pub fn write_byte(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6886Error<E>> {
366        self.i2c.write(self.slave_addr, &[reg, byte])
367            .map_err(Mpu6886Error::I2c)?;
368        // delay disabled for dev build
369        // TODO: check effects with physical unit
370        // self.delay.delay_ms(10u8);
371        Ok(())
372    }
373
374    /// Enables bit n at register address reg
375    pub fn write_bit(&mut self, reg: u8, bit_n: u8, enable: bool) -> Result<(), Mpu6886Error<E>> {
376        let mut byte: [u8; 1] = [0; 1];
377        self.read_bytes(reg, &mut byte)?;
378        bits::set_bit(&mut byte[0], bit_n, enable);
379        Ok(self.write_byte(reg, byte[0])?)
380    }
381
382    /// Write bits data at reg from start_bit to start_bit+length
383    pub fn write_bits(&mut self, reg: u8, start_bit: u8, length: u8, data: u8) -> Result<(), Mpu6886Error<E>> {
384        let mut byte: [u8; 1] = [0; 1];
385        self.read_bytes(reg, &mut byte)?;
386        bits::set_bits(&mut byte[0], start_bit, length, data);
387        Ok(self.write_byte(reg, byte[0])?)
388    }
389
390    /// Read bit n from register
391    fn read_bit(&mut self, reg: u8, bit_n: u8) -> Result<u8, Mpu6886Error<E>> {
392        let mut byte: [u8; 1] = [0; 1];
393        self.read_bytes(reg, &mut byte)?;
394        Ok(bits::get_bit(byte[0], bit_n))
395    }
396
397    /// Read bits at register reg, starting with bit start_bit, until start_bit+length
398    pub fn read_bits(&mut self, reg: u8, start_bit: u8, length: u8) -> Result<u8, Mpu6886Error<E>> {
399        let mut byte: [u8; 1] = [0; 1];
400        self.read_bytes(reg, &mut byte)?;
401        Ok(bits::get_bits(byte[0], start_bit, length))
402    }
403
404    /// Reads byte from register
405    pub fn read_byte(&mut self, reg: u8) -> Result<u8, Mpu6886Error<E>> {
406        let mut byte: [u8; 1] = [0; 1];
407        self.i2c.write_read(self.slave_addr, &[reg], &mut byte)
408            .map_err(Mpu6886Error::I2c)?;
409        Ok(byte[0])
410    }
411
412    /// Reads series of bytes into buf from specified reg
413    pub fn read_bytes(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), Mpu6886Error<E>> {
414        self.i2c.write_read(self.slave_addr, &[reg], buf)
415            .map_err(Mpu6886Error::I2c)?;
416        Ok(())
417    }
418}
419
mpu6886/lib.rs

mpu6886/
lib.rs
