From f36aa2c73bc6bf85895168bfde07979c57dfa6d9 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Thu, 18 Feb 2021 01:30:53 +0100 Subject: [PATCH] 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