From e7c0e5ac2023399d8b701057aa509f85156b332d Mon Sep 17 00:00:00 2001 From: juliangaal Date: Wed, 17 Feb 2021 20:32:16 +0100 Subject: [PATCH 1/3] 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 From f36aa2c73bc6bf85895168bfde07979c57dfa6d9 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Thu, 18 Feb 2021 01:30:53 +0100 Subject: [PATCH 2/3] implement readbit(s), writebit(s) --- examples/bits.rs | 31 ++++++++++ examples/motion_detection.rs | 31 ---------- src/bits.rs | 76 ++++++++++++++++++----- src/{registers.rs => device.rs} | 13 ++-- src/lib.rs | 106 +++++++++++++------------------- src/range.rs | 43 +++++++++++++ src/sensitivity.rs | 40 ++++++++++++ 7 files changed, 225 insertions(+), 115 deletions(-) create mode 100644 examples/bits.rs delete mode 100644 examples/motion_detection.rs rename src/{registers.rs => device.rs} (90%) create mode 100644 src/range.rs create mode 100644 src/sensitivity.rs diff --git a/examples/bits.rs b/examples/bits.rs new file mode 100644 index 0000000..12ab25e --- /dev/null +++ b/examples/bits.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)?; + + println!("{:#?}", mpu.get_gyro_range()); + mpu.set_gyro_range(range::GyroRange::D500)?; + use std::{thread, time}; + let ten_millis = time::Duration::from_millis(1000); + thread::sleep(ten_millis); + println!("{:#?}", mpu.get_gyro_range()); + + loop { + // mpu. + } +} + +// MPU6050_RA_MOT_DETECT_CTRL, 0x69, = 3 +// MPU6050_RA_INT_ENABLE, 0x38, = 1 +// MPU6050_RA_ACCEL_CONFIG, 0x1C, = 1 +// MPU6050_RA_MOT_THR, 0x1F, = 2 +// MPU6050_RA_ZRMOT_DUR, 0x22, = 1 \ No newline at end of file diff --git a/examples/motion_detection.rs b/examples/motion_detection.rs deleted file mode 100644 index b38d143..0000000 --- a/examples/motion_detection.rs +++ /dev/null @@ -1,31 +0,0 @@ -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 index 08b00d4..d273106 100644 --- a/src/bits.rs +++ b/src/bits.rs @@ -1,44 +1,92 @@ // 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 get_bit(byte: u8, n: u8) -> u8 { + (byte >> n) & 1 } -pub fn set_bit_n(byte: &mut [u8; 1], n: u8, enable: bool) { +pub fn get_bits(mut byte: u8, bit_start: u8, length: u8) -> u8 { + // 01101001 read byte + // 76543210 bit numbers + // xxx args: bit_start=4, length=3 + // 010 masked + // -> 010 shifted + let mask: u8 = ((1 << length) - 1) << (bit_start - length + 1); + byte &= mask; + byte >>= bit_start - length + 1; + byte +} + +pub fn set_bit(byte: &mut u8, n: u8, enable: bool) { if enable { - byte[0] |= 1_u8 << n; + *byte |= 1_u8 << n; } else { - byte[0] &= !(1_u8 << n); + *byte &= !(1_u8 << n); } } +pub fn set_bits(byte: &mut u8, bit_start: u8, length: u8, mut data: u8) { + /* + 010 value to write + 76543210 bit numbers + xxx args: bit_start=4, length=3 + 00011100 mask byte + 10101111 original value (sample) + 10100011 original & ~mask + 10101011 masked | value + */ + let mask: u8 = ((1 << length) - 1) << (bit_start - length + 1); + data <<= bit_start - length + 1; // shift data into correct position + data &= mask; // zero all non-important bits in data + *byte &= !(mask); // zero all important bits in existing byte + *byte |= data; // combine data with existing byte +} + #[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); + fn get_bit_test() { + assert_eq!(get_bit(4, 2), 1); + assert_eq!(get_bit(4, 1), 0); + assert_eq!(get_bit(4, 0), 0); } #[test] - fn set_bit_n_test() { + fn set_bit_test() { let mut byte = 4_u8.to_be_bytes(); // enable bit 1 - set_bit_n(&mut byte, 1, true); + set_bit(&mut byte[0], 1, true); assert_eq!(byte[0], 6); // disable bit 1 - set_bit_n(&mut byte, 1, false); + set_bit(&mut byte[0], 1, false); assert_eq!(byte[0], 4); // enable bit 3 - set_bit_n(&mut byte, 3, true); + set_bit(&mut byte[0], 3, true); assert_eq!(byte[0], 12); } + + #[test] + fn set_get_bits_test() { + // 010 value to write + // 76543210 bit numbers + // xxx args: bit_start=4, length=3 + // 00011100 mask byte + // 10101111 original value (sample) + // 10100011 original & ~mask + // 10101011 masked | value + let mut original_value: u8 = 175; + let value: u8 = 2; + let bitstart: u8 = 4; + let length: u8 = 3; + set_bits(&mut original_value, bitstart, length, value); + assert_eq!(original_value, 0b10101011); + + let bits = get_bits(original_value, bitstart, length); + assert_eq!(value, bits); + } } diff --git a/src/registers.rs b/src/device.rs similarity index 90% rename from src/registers.rs rename to src/device.rs index c5f2fe7..175f0a4 100644 --- a/src/registers.rs +++ b/src/device.rs @@ -41,13 +41,12 @@ impl Registers { #[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, -} +pub struct Bits; + impl Bits { - pub fn byte(&self) -> u8 { - *self as u8 - } + /// Accelerometer high pass filter bit: See 4.5 Register 28 + pub const ACCEL_HPF_BIT: u8 = 3; + pub const GYRO_CONFIG_FS_SEL_BIT: u8 = 4; + pub const GYRO_CONFIG_FS_SEL_LENGTH: u8 = 3; } \ No newline at end of file diff --git a/src/lib.rs b/src/lib.rs index 41afa37..b62915e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -41,17 +41,21 @@ #![no_std] -pub mod registers; -mod bits; +pub mod device; +pub mod bits; +pub mod sensitivity; +pub mod range; + +use crate::sensitivity::*; +use crate::range::*; +use crate::device::{Registers::*, Bits}; -use crate::registers::Registers::*; use libm::{powf, atan2f, sqrtf}; use nalgebra::{Vector3, Vector2}; use embedded_hal::{ blocking::delay::DelayMs, blocking::i2c::{Write, WriteRead}, }; -use crate::registers::Registers; /// PI, f32 pub const PI: f32 = core::f32::consts::PI; @@ -59,61 +63,6 @@ pub const PI: f32 = core::f32::consts::PI; /// PI / 180, for conversion to radians pub const PI_180: f32 = PI / 180.0; -/// Gyro Sensitivity -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); - -// Converts accelerometer range to correction/scaling factor, see table p. 29 or register sheet -impl From for Sensitivity { - fn from(range: AccelRange) -> Sensitivity { - match range { - AccelRange::G2 => return Sensitivity(AFS_SEL.0), - AccelRange::G4 => return Sensitivity(AFS_SEL.1), - AccelRange::G8 => return Sensitivity(AFS_SEL.2), - AccelRange::G16 => return Sensitivity(AFS_SEL.3), - } - } -} - -// Converts gyro range to correction/scaling factor, see table p. 31 or register sheet -impl From for Sensitivity { - fn from(range: GyroRange) -> Sensitivity { - match range { - GyroRange::DEG250 => return Sensitivity(FS_SEL.0), - GyroRange::DEG500 => return Sensitivity(FS_SEL.1), - GyroRange::DEG1000 => return Sensitivity(FS_SEL.2), - GyroRange::DEG2000 => return Sensitivity(FS_SEL.3), - } - } -} - -/// Defines accelerometer range/sensivity -pub enum AccelRange { - G2, - G4, - G8, - G16, -} - -/// Defines gyro range/sensitivity -pub enum GyroRange { - DEG250, - DEG500, - DEG1000, - DEG2000, -} - /// All possible errors in this crate #[derive(Debug)] pub enum Mpu6050Error { @@ -139,8 +88,8 @@ where pub fn new(i2c: I) -> Self { Mpu6050 { i2c, - acc_sensitivity: AFS_SEL.0, - gyro_sensitivity: FS_SEL.0, + acc_sensitivity: ACCEL_SENS.0, + gyro_sensitivity: GYRO_SENS.0, } } @@ -176,6 +125,23 @@ where Ok(()) } + pub fn set_gyro_range(&mut self, scale: GyroRange) -> Result<(), Mpu6050Error> { + Ok( + self.write_bits(GYRO_CONFIG.addr(), + Bits::GYRO_CONFIG_FS_SEL_BIT, + Bits::GYRO_CONFIG_FS_SEL_LENGTH, + scale as u8)? + ) + } + + pub fn get_gyro_range(&mut self) -> Result> { + let byte = self.read_bits(GYRO_CONFIG.addr(), + Bits::GYRO_CONFIG_FS_SEL_BIT, + Bits::GYRO_CONFIG_FS_SEL_LENGTH)?; + + Ok(GyroRange::from(byte)) + } + /// Roll and pitch estimation from raw accelerometer readings /// NOTE: no yaw! no magnetometer present on MPU6050 pub fn get_acc_angles(&mut self) -> Result, Mpu6050Error> { @@ -254,7 +220,15 @@ where 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); + bits::set_bit(&mut byte[0], bit_n, enable); + Ok(self.write_byte(reg, byte[0])?) + } + + /// Write bits data at reg from start_bit to start_bit+length + pub fn write_bits(&mut self, reg: u8, start_bit: u8, length: u8, data: u8) -> Result<(), Mpu6050Error> { + let mut byte: [u8; 1] = [0; 1]; + self.read_bytes(reg, &mut byte)?; + bits::set_bits(&mut byte[0], start_bit, length, data); Ok(self.write_byte(reg, byte[0])?) } @@ -262,7 +236,13 @@ where 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)) + Ok(bits::get_bit(byte[0], bit_n)) + } + + pub fn read_bits(&mut self, reg: u8, start_bit: u8, length: u8) -> Result> { + let mut byte: [u8; 1] = [0; 1]; + self.read_bytes(reg, &mut byte)?; + Ok(bits::get_bits(byte[0], start_bit, length)) } /// Reads byte from register diff --git a/src/range.rs b/src/range.rs new file mode 100644 index 0000000..117ac3d --- /dev/null +++ b/src/range.rs @@ -0,0 +1,43 @@ +/// Defines accelerometer range/sensivity +#[derive(Debug)] +pub enum AccelRange { + G2 = 0, + G4, + G8, + G16, +} + +/// Defines gyro range/sensitivity +#[derive(Debug)] +pub enum GyroRange { + D250 = 0, + D500, + D1000, + D2000, +} + +impl From for GyroRange { + fn from(range: u8) -> Self + { + match range { + 0 => GyroRange::D250, + 1 => GyroRange::D500, + 2 => GyroRange::D1000, + 3 => GyroRange::D2000, + _ => GyroRange::D250 + } + } +} + +impl From for AccelRange { + fn from(range: u8) -> Self + { + match range { + 0 => AccelRange::G2, + 1 => AccelRange::G4, + 2 => AccelRange::G8, + 3 => AccelRange::G16, + _ => AccelRange::G2 + } + } +} \ No newline at end of file diff --git a/src/sensitivity.rs b/src/sensitivity.rs new file mode 100644 index 0000000..59503fe --- /dev/null +++ b/src/sensitivity.rs @@ -0,0 +1,40 @@ +use crate::range::*; + +/// Gyro Sensitivity +pub const GYRO_SENS: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4); + +/// Accelerometer Sensitivity +pub const ACCEL_SENS: (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 +pub(crate) struct Sensitivity(pub(crate) f32); + +// Converts accelerometer range to correction/scaling factor, see table p. 29 or register sheet +impl From for Sensitivity { + fn from(range: AccelRange) -> Sensitivity { + match range { + AccelRange::G2 => return Sensitivity(ACCEL_SENS.0), + AccelRange::G4 => return Sensitivity(ACCEL_SENS.1), + AccelRange::G8 => return Sensitivity(ACCEL_SENS.2), + AccelRange::G16 => return Sensitivity(ACCEL_SENS.3), + } + } +} + +// Converts gyro range to correction/scaling factor, see table p. 31 or register sheet +impl From for Sensitivity { + fn from(range: GyroRange) -> Self { + match range { + GyroRange::D250 => return Sensitivity(GYRO_SENS.0), + GyroRange::D500 => return Sensitivity(GYRO_SENS.1), + GyroRange::D1000 => return Sensitivity(GYRO_SENS.2), + GyroRange::D2000 => return Sensitivity(GYRO_SENS.3), + } + } +} \ No newline at end of file From ea54b0accf9780bc1e3d65b2b97cf3a6133bf2da Mon Sep 17 00:00:00 2001 From: juliangaal Date: Thu, 18 Feb 2021 12:27:26 +0100 Subject: [PATCH 3/3] set range, hpf --- examples/bits.rs | 31 --------------------- examples/test.rs | 33 +++++++++++++++++++++++ src/bits.rs | 46 ++++++++++++++++++++++++++----- src/device.rs | 64 ++++++++++++++++++++++++++++++++++++++++++- src/lib.rs | 67 +++++++++++++++++++++++++++++++++++++--------- src/range.rs | 29 ++++++++++++++++++-- src/sensitivity.rs | 40 --------------------------- 7 files changed, 217 insertions(+), 93 deletions(-) delete mode 100644 examples/bits.rs create mode 100644 examples/test.rs delete mode 100644 src/sensitivity.rs diff --git a/examples/bits.rs b/examples/bits.rs deleted file mode 100644 index 12ab25e..0000000 --- a/examples/bits.rs +++ /dev/null @@ -1,31 +0,0 @@ -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)?; - - println!("{:#?}", mpu.get_gyro_range()); - mpu.set_gyro_range(range::GyroRange::D500)?; - use std::{thread, time}; - let ten_millis = time::Duration::from_millis(1000); - thread::sleep(ten_millis); - println!("{:#?}", mpu.get_gyro_range()); - - loop { - // mpu. - } -} - -// MPU6050_RA_MOT_DETECT_CTRL, 0x69, = 3 -// MPU6050_RA_INT_ENABLE, 0x38, = 1 -// MPU6050_RA_ACCEL_CONFIG, 0x1C, = 1 -// MPU6050_RA_MOT_THR, 0x1F, = 2 -// MPU6050_RA_ZRMOT_DUR, 0x22, = 1 \ No newline at end of file diff --git a/examples/test.rs b/examples/test.rs new file mode 100644 index 0000000..1e84dcd --- /dev/null +++ b/examples/test.rs @@ -0,0 +1,33 @@ +use mpu6050::*; + +use linux_embedded_hal::{I2cdev, Delay}; +use i2cdev::linux::LinuxI2CError; +use mpu6050::device::ACCEL_HPF; + +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)?; + + // Test gyro config + assert_eq!(mpu.get_gyro_range()?, range::GyroRange::D250); + mpu.set_gyro_range(range::GyroRange::D500)?; + assert_eq!(mpu.get_gyro_range()?, range::GyroRange::D500); + + // Test accel config + assert_eq!(mpu.get_accel_range()?, range::AccelRange::G2); + mpu.set_accel_range(range::AccelRange::G4)?; + assert_eq!(mpu.get_accel_range()?, range::AccelRange::G4); + + // accel_hpf + assert_eq!(mpu.get_accel_hpf()?, ACCEL_HPF::_RESET); + mpu.set_accel_hpf(ACCEL_HPF::_1P25); + assert_eq!(mpu.get_accel_hpf()?, ACCEL_HPF::_1P25); + + println!("Test successful"); + Ok(()) +} \ No newline at end of file diff --git a/src/bits.rs b/src/bits.rs index d273106..a9fa8d2 100644 --- a/src/bits.rs +++ b/src/bits.rs @@ -1,22 +1,33 @@ -// Mostly taken from https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/I2Cdev/I2Cdev.cpp -// and tested +//! Bit operations on registers +//! Mostly taken from https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/I2Cdev/I2Cdev.cpp +//! updated and tested +/// get bit n of byte pub fn get_bit(byte: u8, n: u8) -> u8 { (byte >> n) & 1 } +/// get bits start - start+length from byte pub fn get_bits(mut byte: u8, bit_start: u8, length: u8) -> u8 { // 01101001 read byte // 76543210 bit numbers // xxx args: bit_start=4, length=3 // 010 masked // -> 010 shifted - let mask: u8 = ((1 << length) - 1) << (bit_start - length + 1); - byte &= mask; - byte >>= bit_start - length + 1; + + // without mask_shift, strange behavior occurs, wenn bit_start < length. + // e.g. bit_start=2, length = 2 + // in SOME cases, you get an 'attempt to subtract with overflow' exception, when + // bitstart - length + 1 = 0 + // therefore just "cut off" at 0 shift + let mask_shift: u8 = if bit_start < length { 0 } else { bit_start - length + 1 }; + let mask: u8 = ((1 << length) - 1) << mask_shift; + byte &= mask as u8; + byte >>= mask_shift; byte } +/// set bit n in byte pub fn set_bit(byte: &mut u8, n: u8, enable: bool) { if enable { *byte |= 1_u8 << n; @@ -25,6 +36,7 @@ pub fn set_bit(byte: &mut u8, n: u8, enable: bool) { } } +/// Fill bits bitstart-bitstart+length in byte with data pub fn set_bits(byte: &mut u8, bit_start: u8, length: u8, mut data: u8) { /* 010 value to write @@ -35,8 +47,15 @@ pub fn set_bits(byte: &mut u8, bit_start: u8, length: u8, mut data: u8) { 10100011 original & ~mask 10101011 masked | value */ - let mask: u8 = ((1 << length) - 1) << (bit_start - length + 1); - data <<= bit_start - length + 1; // shift data into correct position + + // without mask_shift, strange behavior occurs, wenn bit_start < length. + // e.g. bit_start=2, length = 2 + // in SOME cases, you get an 'attempt to subtract with overflow' exception, when + // bitstart - length + 1 = 0 + // therefore just "cut off" at 0 shift + let mask_shift: u8 = if bit_start < length { 0 } else { bit_start - length + 1 }; + let mask: u8 = ((1 << length) - 1) << mask_shift; + data <<= mask_shift; // shift data into correct position data &= mask; // zero all non-important bits in data *byte &= !(mask); // zero all important bits in existing byte *byte |= data; // combine data with existing byte @@ -45,6 +64,10 @@ pub fn set_bits(byte: &mut u8, bit_start: u8, length: u8, mut data: u8) { #[cfg(test)] mod tests { use super::*; + extern crate std; + use std::*; + + use crate::device::*; #[test] fn get_bit_test() { @@ -88,5 +111,14 @@ mod tests { let bits = get_bits(original_value, bitstart, length); assert_eq!(value, bits); + + // simulate accel_hpf + let bitstart = Bits::ACCEL_CONFIG_ACCEL_HPF_BIT; + let length = Bits::ACCEL_CONFIG_ACCEL_HPF_LENGTH; + assert_eq!(get_bits(original_value, bitstart, length), 0b00000011); + + let mode: u8 = 7; + set_bits(&mut original_value, bitstart, length, mode); + assert_eq!(get_bits(original_value, bitstart, length), 0b00000111); } } diff --git a/src/device.rs b/src/device.rs index 175f0a4..7de93bf 100644 --- a/src/device.rs +++ b/src/device.rs @@ -2,8 +2,22 @@ //! 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 //! + +/// Gyro Sensitivity +pub const GYRO_SENS: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4); + +/// Accelerometer Sensitivity +pub const ACCEL_SENS: (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.; + #[allow(non_camel_case_types)] #[derive(Copy, Clone, Debug)] +/// Register addresses pub enum Registers { /// Slave address of Mpu6050 SLAVE_ADDR = 0x68, @@ -47,6 +61,54 @@ pub struct Bits; impl Bits { /// Accelerometer high pass filter bit: See 4.5 Register 28 pub const ACCEL_HPF_BIT: u8 = 3; + + /// Gyro Config FS_SEL start bit pub const GYRO_CONFIG_FS_SEL_BIT: u8 = 4; + /// Gyro Config FS_SEL length pub const GYRO_CONFIG_FS_SEL_LENGTH: u8 = 3; -} \ No newline at end of file + + + /// Accel Config FS_SEL start bit + pub const ACCEL_CONFIG_FS_SEL_BIT: u8 = 4; + /// Accel Config FS_SEL length + pub const ACCEL_CONFIG_FS_SEL_LENGTH: u8 = 2; + /// Accel Config FS_SEL start bit + pub const ACCEL_CONFIG_ACCEL_HPF_BIT: u8 = 2; + /// Accel Config FS_SEL length + pub const ACCEL_CONFIG_ACCEL_HPF_LENGTH: u8 = 3; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug, Eq, PartialEq)] +pub enum ACCEL_HPF { + _RESET = 0, + _5 = 1, + _2P5 = 2, + _1P25 = 3, + _0P63 = 4, + _HOLD = 7 +} + +impl From for ACCEL_HPF { + fn from(range: u8) -> Self + { + match range { + 0 => ACCEL_HPF::_RESET, + 1 => ACCEL_HPF::_5, + 2 => ACCEL_HPF::_2P5, + 3 => ACCEL_HPF::_1P25, + 4 => ACCEL_HPF::_0P63, + 7 => ACCEL_HPF::_HOLD, + _ => ACCEL_HPF::_RESET, + } + } +} +// +// #[derive(Copy, Clone, Debug)] +// pub struct BitBlock { +// reg: u8, +// start_bit: u8, +// length: u8 +// } +// +// pub const ACONFIG_ACCEL_HBF: BitBlock = BitBlock { reg: Registers::ACCEL_CONFIG.addr(), start_bit: Bits::ACCEL_CONFIG_ACCEL_HBF_BIT, length: Bits::ACCEL_CONFIG_ACCEL_HBF_LENGTH}; \ No newline at end of file diff --git a/src/lib.rs b/src/lib.rs index b62915e..cfb8d26 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -43,12 +43,10 @@ pub mod device; pub mod bits; -pub mod sensitivity; pub mod range; -use crate::sensitivity::*; use crate::range::*; -use crate::device::{Registers::*, Bits}; +use crate::device::{*, Registers::*, Bits}; use libm::{powf, atan2f, sqrtf}; use nalgebra::{Vector3, Vector2}; @@ -97,8 +95,8 @@ where pub fn new_with_sens(i2c: I, arange: AccelRange, grange: GyroRange) -> Self { Mpu6050 { i2c, - acc_sensitivity: Sensitivity::from(arange).0, - gyro_sensitivity: Sensitivity::from(grange).0, + acc_sensitivity: arange.sensitivity(), + gyro_sensitivity: grange.sensitivity(), } } @@ -113,6 +111,9 @@ where pub fn init>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error> { self.wake(delay)?; self.verify()?; + self.set_accel_range(AccelRange::G2)?; + self.set_gyro_range(GyroRange::D250)?; + self.set_accel_hpf(ACCEL_HPF::_RESET)?; Ok(()) } @@ -125,19 +126,60 @@ where Ok(()) } - pub fn set_gyro_range(&mut self, scale: GyroRange) -> Result<(), Mpu6050Error> { + /// set accel high pass filter mode + pub fn set_accel_hpf(&mut self, mode: ACCEL_HPF) -> Result<(), Mpu6050Error> { Ok( - self.write_bits(GYRO_CONFIG.addr(), - Bits::GYRO_CONFIG_FS_SEL_BIT, - Bits::GYRO_CONFIG_FS_SEL_LENGTH, - scale as u8)? + self.write_bits(ACCEL_CONFIG.addr(), + Bits::ACCEL_CONFIG_ACCEL_HPF_BIT, + Bits::ACCEL_CONFIG_ACCEL_HPF_LENGTH, mode as u8)? ) } + /// get accel high pass filter mode + pub fn get_accel_hpf(&mut self) -> Result> { + let mode: u8 = self.read_bits(ACCEL_CONFIG.addr(), + Bits::ACCEL_CONFIG_ACCEL_HPF_BIT, + Bits::ACCEL_CONFIG_ACCEL_HPF_LENGTH)?; + + Ok(ACCEL_HPF::from(mode)) + } + + /// Set gyro range, and update sensitivity accordingly + pub fn set_gyro_range(&mut self, range: GyroRange) -> Result<(), Mpu6050Error> { + self.write_bits(GYRO_CONFIG.addr(), + Bits::GYRO_CONFIG_FS_SEL_BIT, + Bits::GYRO_CONFIG_FS_SEL_LENGTH, + range as u8)?; + + self.gyro_sensitivity = range.sensitivity(); + Ok(()) + } + + /// set accel range, and update sensitivy accordingly + pub fn set_accel_range(&mut self, range: AccelRange) -> Result<(), Mpu6050Error> { + self.write_bits(ACCEL_CONFIG.addr(), + Bits::ACCEL_CONFIG_FS_SEL_BIT, + Bits::ACCEL_CONFIG_FS_SEL_LENGTH, + range as u8)?; + + self.acc_sensitivity = range.sensitivity(); + Ok(()) + } + + /// get current accel_range + pub fn get_accel_range(&mut self) -> Result> { + let byte = self.read_bits(ACCEL_CONFIG.addr(), + Bits::ACCEL_CONFIG_FS_SEL_BIT, + Bits::ACCEL_CONFIG_FS_SEL_LENGTH)?; + + Ok(AccelRange::from(byte)) + } + + /// get current gyro range pub fn get_gyro_range(&mut self) -> Result> { let byte = self.read_bits(GYRO_CONFIG.addr(), - Bits::GYRO_CONFIG_FS_SEL_BIT, - Bits::GYRO_CONFIG_FS_SEL_LENGTH)?; + Bits::GYRO_CONFIG_FS_SEL_BIT, + Bits::GYRO_CONFIG_FS_SEL_LENGTH)?; Ok(GyroRange::from(byte)) } @@ -239,6 +281,7 @@ where Ok(bits::get_bit(byte[0], bit_n)) } + /// Read bits at register reg, starting with bit start_bit, until start_bit+length pub fn read_bits(&mut self, reg: u8, start_bit: u8, length: u8) -> Result> { let mut byte: [u8; 1] = [0; 1]; self.read_bytes(reg, &mut byte)?; diff --git a/src/range.rs b/src/range.rs index 117ac3d..6bc0698 100644 --- a/src/range.rs +++ b/src/range.rs @@ -1,5 +1,7 @@ +use crate::device::*; + /// Defines accelerometer range/sensivity -#[derive(Debug)] +#[derive(Debug, Eq, PartialEq, Copy, Clone)] pub enum AccelRange { G2 = 0, G4, @@ -8,7 +10,7 @@ pub enum AccelRange { } /// Defines gyro range/sensitivity -#[derive(Debug)] +#[derive(Debug, Eq, PartialEq, Copy, Clone)] pub enum GyroRange { D250 = 0, D500, @@ -40,4 +42,27 @@ impl From for AccelRange { _ => AccelRange::G2 } } +} + +impl AccelRange { + pub(crate) fn sensitivity(&self) -> f32 { + match &self { + AccelRange::G2 => ACCEL_SENS.0, + AccelRange::G4 => ACCEL_SENS.1, + AccelRange::G8 => ACCEL_SENS.2, + AccelRange::G16 => ACCEL_SENS.3, + } + } +} + +// Converts gyro range to correction/scaling factor, see table p. 31 or register sheet +impl GyroRange { + pub(crate) fn sensitivity(&self) -> f32 { + match &self { + GyroRange::D250 => GYRO_SENS.0, + GyroRange::D500 => GYRO_SENS.1, + GyroRange::D1000 => GYRO_SENS.2, + GyroRange::D2000 => GYRO_SENS.3, + } + } } \ No newline at end of file diff --git a/src/sensitivity.rs b/src/sensitivity.rs deleted file mode 100644 index 59503fe..0000000 --- a/src/sensitivity.rs +++ /dev/null @@ -1,40 +0,0 @@ -use crate::range::*; - -/// Gyro Sensitivity -pub const GYRO_SENS: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4); - -/// Accelerometer Sensitivity -pub const ACCEL_SENS: (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 -pub(crate) struct Sensitivity(pub(crate) f32); - -// Converts accelerometer range to correction/scaling factor, see table p. 29 or register sheet -impl From for Sensitivity { - fn from(range: AccelRange) -> Sensitivity { - match range { - AccelRange::G2 => return Sensitivity(ACCEL_SENS.0), - AccelRange::G4 => return Sensitivity(ACCEL_SENS.1), - AccelRange::G8 => return Sensitivity(ACCEL_SENS.2), - AccelRange::G16 => return Sensitivity(ACCEL_SENS.3), - } - } -} - -// Converts gyro range to correction/scaling factor, see table p. 31 or register sheet -impl From for Sensitivity { - fn from(range: GyroRange) -> Self { - match range { - GyroRange::D250 => return Sensitivity(GYRO_SENS.0), - GyroRange::D500 => return Sensitivity(GYRO_SENS.1), - GyroRange::D1000 => return Sensitivity(GYRO_SENS.2), - GyroRange::D2000 => return Sensitivity(GYRO_SENS.3), - } - } -} \ No newline at end of file