diff --git a/examples/linux.rs b/examples/simple.rs similarity index 100% rename from examples/linux.rs rename to examples/simple.rs 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 new file mode 100644 index 0000000..a9fa8d2 --- /dev/null +++ b/src/bits.rs @@ -0,0 +1,124 @@ +//! 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 + + // 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; + } else { + *byte &= !(1_u8 << n); + } +} + +/// 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 + 76543210 bit numbers + xxx args: bit_start=4, length=3 + 00011100 mask byte + 10101111 original value (sample) + 10100011 original & ~mask + 10101011 masked | value + */ + + // 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 +} + +#[cfg(test)] +mod tests { + use super::*; + extern crate std; + use std::*; + + use crate::device::*; + + #[test] + 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_test() { + let mut byte = 4_u8.to_be_bytes(); + + // enable bit 1 + set_bit(&mut byte[0], 1, true); + assert_eq!(byte[0], 6); + + // disable bit 1 + set_bit(&mut byte[0], 1, false); + assert_eq!(byte[0], 4); + + // enable bit 3 + 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); + + // 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 new file mode 100644 index 0000000..7de93bf --- /dev/null +++ b/src/device.rs @@ -0,0 +1,114 @@ +//! 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 +//! + +/// 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, + /// Internal register to check slave addr + WHOAMI = 0x75, + /// High Byte Register Gyro x orientation + GYRO_REGX_H = 0x43, + /// High Byte Register Gyro y orientation + GYRO_REGY_H = 0x45, + /// High Byte Register Gyro z orientation + GYRO_REGZ_H = 0x47, + /// High Byte Register Calc roll + ACC_REGX_H = 0x3b, + /// High Byte Register Calc pitch + ACC_REGY_H = 0x3d, + /// High Byte Register Calc yaw + ACC_REGZ_H = 0x3f, + /// High Byte Register Temperature + TEMP_OUT_H = 0x41, + /// Register to control chip waking from sleep, enabling sensors, default: sleep + POWER_MGMT_1 = 0x6b, + /// Internal clock + POWER_MGMT_2 = 0x6c, + /// Accelerometer config register + ACCEL_CONFIG = 0x1c, + /// gyro config register + GYRO_CONFIG = 0x1b, +} + +impl Registers { + pub fn addr(&self) -> u8 { + *self as u8 + } +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +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 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 139cdba..cfb8d26 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -41,9 +41,13 @@ #![no_std] -pub mod registers; +pub mod device; +pub mod bits; +pub mod range; + +use crate::range::*; +use crate::device::{*, Registers::*, Bits}; -use crate::registers::Registers::*; use libm::{powf, atan2f, sqrtf}; use nalgebra::{Vector3, Vector2}; use embedded_hal::{ @@ -57,55 +61,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.); - -// 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 { @@ -131,8 +86,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, } } @@ -140,14 +95,14 @@ 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(), } } /// 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(()) } @@ -156,18 +111,79 @@ 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(()) } /// 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)); } Ok(()) } + /// set accel high pass filter mode + pub fn set_accel_hpf(&mut self, mode: ACCEL_HPF) -> Result<(), Mpu6050Error> { + Ok( + 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)?; + + 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> { @@ -228,11 +244,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 +257,39 @@ 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(&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])?) + } + + /// 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(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)?; + Ok(bits::get_bits(byte[0], start_bit, length)) + } + /// 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 +303,4 @@ where Ok(()) } } + diff --git a/src/range.rs b/src/range.rs new file mode 100644 index 0000000..6bc0698 --- /dev/null +++ b/src/range.rs @@ -0,0 +1,68 @@ +use crate::device::*; + +/// Defines accelerometer range/sensivity +#[derive(Debug, Eq, PartialEq, Copy, Clone)] +pub enum AccelRange { + G2 = 0, + G4, + G8, + G16, +} + +/// Defines gyro range/sensitivity +#[derive(Debug, Eq, PartialEq, Copy, Clone)] +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 + } + } +} + +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/registers.rs b/src/registers.rs deleted file mode 100644 index f777c40..0000000 --- a/src/registers.rs +++ /dev/null @@ -1,38 +0,0 @@ -//! All constants used in the driver, mostly register addresses - -#[allow(non_camel_case_types)] -#[derive(Copy, Clone, Debug)] -pub enum Registers { - /// Slave address of Mpu6050 - SLAVE_ADDR = 0x68, - /// Internal register to check slave addr - WHOAMI = 0x75, - /// High Byte Register Gyro x orientation - GYRO_REGX_H = 0x43, - /// High Byte Register Gyro y orientation - GYRO_REGY_H = 0x45, - /// High Byte Register Gyro z orientation - GYRO_REGZ_H = 0x47, - /// High Byte Register Calc roll - ACC_REGX_H = 0x3b, - /// High Byte Register Calc pitch - ACC_REGY_H = 0x3d, - /// High Byte Register Calc yaw - ACC_REGZ_H = 0x3f, - /// High Byte Register Temperature - TEMP_OUT_H = 0x41, - /// Register to control chip waking from sleep, enabling sensors, default: sleep - POWER_MGMT_1 = 0x6b, - /// Internal clock - POWER_MGMT_2 = 0x6c, - /// Accelerometer config register - ACCEL_CONFIG = 0x1c, - /// gyro config register - GYRO_CONFIG = 0x1b, -} - -impl Registers { - pub fn addr(&self) -> u8 { - *self as u8 - } -}