implement readbit(s), writebit(s)

examples/bits.rs

@@ -0,0 +1,31 @@
+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/motion_detection.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)?;
-
-    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);
-    }
-}

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 {
+pub fn get_bit(byte: u8, n: u8) -> u8 {
-    (byte[0] >> n) & 1
+    (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() {
+    fn get_bit_test() {
-        let byte = 4_u8.to_be_bytes();
+        assert_eq!(get_bit(4, 2), 1);
-        assert_eq!(get_bit_n(&byte, 2), 1);
+        assert_eq!(get_bit(4, 1), 0);
-        assert_eq!(get_bit_n(&byte, 1), 0);
+        assert_eq!(get_bit(4, 0), 0);
-        assert_eq!(get_bit_n(&byte, 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);
+    }
 }

src/device.rs

@@ -41,13 +41,12 @@ impl Registers {
 
 #[allow(non_camel_case_types)]
 #[derive(Copy, Clone, Debug)]
-pub enum Bits {
+pub struct Bits;
-    /// Accelerometer high pass filter bit: See 4.5 Register 28
+
-    ACCEL_HPF_BIT = 2,
-}
 
 impl Bits {
-    pub fn byte(&self) -> u8 {
+    /// Accelerometer high pass filter bit: See 4.5 Register 28
-        *self as u8
+    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;
 }

src/lib.rs

@@ -41,17 +41,21 @@
 
 #![no_std]
 
-pub mod registers;
+pub mod device;
-mod bits;
+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<AccelRange> 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<GyroRange> 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<E> {
@@ -139,8 +88,8 @@ where
     pub fn new(i2c: I) -> Self {
         Mpu6050 {
             i2c,
-            acc_sensitivity: AFS_SEL.0,
+            acc_sensitivity: ACCEL_SENS.0,
-            gyro_sensitivity: FS_SEL.0, 
+            gyro_sensitivity: GYRO_SENS.0, 
         }
     }
 
@@ -176,6 +125,23 @@ where
         Ok(())
     }
 
+    pub fn set_gyro_range(&mut self, scale: GyroRange) -> Result<(), Mpu6050Error<E>> {
+        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<GyroRange, Mpu6050Error<E>> {
+        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<Vector2<f32>, Mpu6050Error<E>> {
@@ -254,7 +220,15 @@ where
     pub fn write_bit(&mut self, reg: u8, bit_n: u8, enable: bool) -> Result<(), Mpu6050Error<E>> {
         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<E>> {
+        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<u8, Mpu6050Error<E>> {
         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<u8, Mpu6050Error<E>> {
+        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

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<u8> 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<u8> for AccelRange {
+    fn from(range: u8) -> Self
+    {
+        match range {
+            0 => AccelRange::G2,
+            1 => AccelRange::G4,
+            2 => AccelRange::G8,
+            3 => AccelRange::G16,
+            _ => AccelRange::G2
+        }
+    }
+}

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<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),
+        }
+    }
+}