set range, hpf

examples/bits.rs

@@ -1,31 +0,0 @@
-use mpu6050::*;
-
-use linux_embedded_hal::{I2cdev, Delay};
-use i2cdev::linux::LinuxI2CError;
-
-fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
-    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

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<LinuxI2CError>> {
+    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(())
+}

src/bits.rs

@@ -1,22 +1,33 @@
-// Mostly taken from https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/I2Cdev/I2Cdev.cpp
+//! Bit operations on registers
-// and tested
+//! 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;
+    // without mask_shift, strange behavior occurs, wenn bit_start < length.
-    byte >>= bit_start - length + 1;
+    // 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);
     }
 }

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;
+
+
+    /// 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<u8> 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};

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,
+            acc_sensitivity: arange.sensitivity(),
-            gyro_sensitivity: Sensitivity::from(grange).0,
+            gyro_sensitivity: grange.sensitivity(),
         }
     }
 
@@ -113,6 +111,9 @@ where
     pub fn init<D: DelayMs<u8>>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error<E>> {
         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,15 +126,56 @@ where
         Ok(())
     }
 
-    pub fn set_gyro_range(&mut self, scale: GyroRange) -> Result<(), Mpu6050Error<E>> {
+    /// set accel high pass filter mode
+    pub fn set_accel_hpf(&mut self, mode: ACCEL_HPF) -> Result<(), Mpu6050Error<E>> {
         Ok(
-            self.write_bits(GYRO_CONFIG.addr(),
+            self.write_bits(ACCEL_CONFIG.addr(),
-                            Bits::GYRO_CONFIG_FS_SEL_BIT,
+                        Bits::ACCEL_CONFIG_ACCEL_HPF_BIT,
-                            Bits::GYRO_CONFIG_FS_SEL_LENGTH,
+                        Bits::ACCEL_CONFIG_ACCEL_HPF_LENGTH, mode as u8)?
-                            scale as u8)?
         )
     }
 
+    /// get accel high pass filter mode
+    pub fn get_accel_hpf(&mut self) -> Result<ACCEL_HPF, Mpu6050Error<E>> {
+        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<E>> {
+        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<E>> {
+        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<AccelRange, Mpu6050Error<E>> {
+        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<GyroRange, Mpu6050Error<E>> {
         let byte = self.read_bits(GYRO_CONFIG.addr(),
                                   Bits::GYRO_CONFIG_FS_SEL_BIT,
@@ -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<u8, Mpu6050Error<E>> {
         let mut byte: [u8; 1] = [0; 1];
         self.read_bytes(reg, &mut byte)?;

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,
@@ -41,3 +43,26 @@ impl From<u8> for AccelRange {
         }
     }
 }
+
+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,
+        }
+    }
+}

src/sensitivity.rs

@@ -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<AccelRange> 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<GyroRange> 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),
-        }
-    }
-}