From e7c0e5ac2023399d8b701057aa509f85156b332d Mon Sep 17 00:00:00 2001 From: juliangaal Date: Wed, 17 Feb 2021 20:32:16 +0100 Subject: [PATCH] write_bit_n, set_bit_n implemented --- examples/{linux.rs => motion_detection.rs} | 0 examples/simple.rs | 31 +++++++++++++++ src/bits.rs | 44 ++++++++++++++++++++++ src/lib.rs | 37 +++++++++++++++--- src/registers.rs | 19 +++++++++- 5 files changed, 123 insertions(+), 8 deletions(-) rename examples/{linux.rs => motion_detection.rs} (100%) create mode 100644 examples/simple.rs create mode 100644 src/bits.rs diff --git a/examples/linux.rs b/examples/motion_detection.rs similarity index 100% rename from examples/linux.rs rename to examples/motion_detection.rs diff --git a/examples/simple.rs b/examples/simple.rs new file mode 100644 index 0000000..b38d143 --- /dev/null +++ b/examples/simple.rs @@ -0,0 +1,31 @@ +use mpu6050::*; +use linux_embedded_hal::{I2cdev, Delay}; +use i2cdev::linux::LinuxI2CError; + +fn main() -> Result<(), Mpu6050Error> { + let i2c = I2cdev::new("/dev/i2c-1") + .map_err(Mpu6050Error::I2c)?; + + let mut delay = Delay; + let mut mpu = Mpu6050::new(i2c); + + mpu.init(&mut delay)?; + + loop { + // get roll and pitch estimate + let acc = mpu.get_acc_angles()?; + println!("r/p: {:?}", acc); + + // get temp + let temp = mpu.get_temp()?; + println!("temp: {:?}c", temp); + + // get gyro data, scaled with sensitivity + let gyro = mpu.get_gyro()?; + println!("gyro: {:?}", gyro); + + // get accelerometer data, scaled with sensitivity + let acc = mpu.get_acc()?; + println!("acc: {:?}", acc); + } +} diff --git a/src/bits.rs b/src/bits.rs new file mode 100644 index 0000000..08b00d4 --- /dev/null +++ b/src/bits.rs @@ -0,0 +1,44 @@ +// Mostly taken from https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/I2Cdev/I2Cdev.cpp +// and tested + +pub fn get_bit_n(byte: &[u8; 1], n: u8) -> u8 { + (byte[0] >> n) & 1 +} + +pub fn set_bit_n(byte: &mut [u8; 1], n: u8, enable: bool) { + if enable { + byte[0] |= 1_u8 << n; + } else { + byte[0] &= !(1_u8 << n); + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn get_bit_n_test() { + let byte = 4_u8.to_be_bytes(); + assert_eq!(get_bit_n(&byte, 2), 1); + assert_eq!(get_bit_n(&byte, 1), 0); + assert_eq!(get_bit_n(&byte, 0), 0); + } + + #[test] + fn set_bit_n_test() { + let mut byte = 4_u8.to_be_bytes(); + + // enable bit 1 + set_bit_n(&mut byte, 1, true); + assert_eq!(byte[0], 6); + + // disable bit 1 + set_bit_n(&mut byte, 1, false); + assert_eq!(byte[0], 4); + + // enable bit 3 + set_bit_n(&mut byte, 3, true); + assert_eq!(byte[0], 12); + } +} diff --git a/src/lib.rs b/src/lib.rs index 139cdba..41afa37 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -42,6 +42,7 @@ #![no_std] pub mod registers; +mod bits; use crate::registers::Registers::*; use libm::{powf, atan2f, sqrtf}; @@ -50,6 +51,7 @@ use embedded_hal::{ blocking::delay::DelayMs, blocking::i2c::{Write, WriteRead}, }; +use crate::registers::Registers; /// PI, f32 pub const PI: f32 = core::f32::consts::PI; @@ -63,6 +65,12 @@ pub const FS_SEL: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4); /// Accelerometer Sensitivity pub const AFS_SEL: (f32, f32, f32, f32) = (16384., 8192., 4096., 2048.); +/// Temperature Offset +pub const TEMP_OFFSET: f32 = 36.53; + +/// Temperature Sensitivity +pub const TEMP_SENSITIVITY: f32 = 340.; + // Helper struct to convert Sensor measurement range to appropriate values defined in datasheet struct Sensitivity(f32); @@ -147,7 +155,7 @@ where /// Wakes MPU6050 with all sensors enabled (default) fn wake>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error> { - self.write_u8(POWER_MGMT_1.addr(), 0)?; + self.write_byte(POWER_MGMT_1.addr(), 0)?; delay.delay_ms(100u8); Ok(()) } @@ -161,7 +169,7 @@ where /// Verifies device to address 0x68 with WHOAMI.addr() Register fn verify(&mut self) -> Result<(), Mpu6050Error> { - let address = self.read_u8(WHOAMI.addr())?; + let address = self.read_byte(WHOAMI.addr())?; if address != SLAVE_ADDR.addr() { return Err(Mpu6050Error::InvalidChipId(address)); } @@ -228,11 +236,12 @@ where self.read_bytes(TEMP_OUT_H.addr(), &mut buf)?; let raw_temp = self.read_word_2c(&buf[0..2]) as f32; - Ok((raw_temp / 340.) + 36.53) + // According to revision 4.2 + Ok((raw_temp / TEMP_SENSITIVITY) + TEMP_OFFSET) } /// Writes byte to register - pub fn write_u8(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6050Error> { + pub fn write_byte(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6050Error> { self.i2c.write(SLAVE_ADDR.addr(), &[reg, byte]) .map_err(Mpu6050Error::I2c)?; // delay disabled for dev build @@ -240,9 +249,24 @@ where // self.delay.delay_ms(10u8); Ok(()) } - + + /// Enables bit n at register address reg + pub fn write_bit(&mut self, reg: u8, bit_n: u8, enable: bool) -> Result<(), Mpu6050Error> { + let mut byte: [u8; 1] = [0; 1]; + self.read_bytes(reg, &mut byte)?; + bits::set_bit_n(byte[0], bit_n, enable); + Ok(self.write_byte(reg, byte[0])?) + } + + /// Read bit n from register + fn read_bit(&mut self, reg: u8, bit_n: u8) -> Result> { + let mut byte: [u8; 1] = [0; 1]; + self.read_bytes(reg, &mut byte)?; + Ok(bits::get_bit_n(&byte, bit_n)) + } + /// Reads byte from register - pub fn read_u8(&mut self, reg: u8) -> Result> { + pub fn read_byte(&mut self, reg: u8) -> Result> { let mut byte: [u8; 1] = [0; 1]; self.i2c.write_read(SLAVE_ADDR.addr(), &[reg], &mut byte) .map_err(Mpu6050Error::I2c)?; @@ -256,3 +280,4 @@ where Ok(()) } } + diff --git a/src/registers.rs b/src/registers.rs index f777c40..c5f2fe7 100644 --- a/src/registers.rs +++ b/src/registers.rs @@ -1,5 +1,7 @@ //! All constants used in the driver, mostly register addresses - +//! Register map: https://arduino.ua/docs/RM-MPU-6000A.pdf +//! Datasheet with WAY more info about interrupts (Revision 3.2) https://www.cdiweb.com/datasheets/invensense/ps-mpu-6000a.pdf +//! #[allow(non_camel_case_types)] #[derive(Copy, Clone, Debug)] pub enum Registers { @@ -34,5 +36,18 @@ pub enum Registers { impl Registers { pub fn addr(&self) -> u8 { *self as u8 - } + } } + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +pub enum Bits { + /// Accelerometer high pass filter bit: See 4.5 Register 28 + ACCEL_HPF_BIT = 2, +} + +impl Bits { + pub fn byte(&self) -> u8 { + *self as u8 + } +} \ No newline at end of file