From ea54b0accf9780bc1e3d65b2b97cf3a6133bf2da Mon Sep 17 00:00:00 2001 From: juliangaal Date: Thu, 18 Feb 2021 12:27:26 +0100 Subject: [PATCH] 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