From eb4a3d31463f41c55cc2acb3ce1734fbda997c2d Mon Sep 17 00:00:00 2001 From: juliangaal Date: Fri, 19 Feb 2021 22:57:04 +0100 Subject: [PATCH 1/4] enable motion detection --- examples/motion_detection.rs | 34 +++++++ misc/status.md | 40 ++++---- src/device.rs | 177 +++++++++++++++++++++++++++++++++-- src/lib.rs | 23 ++++- 4 files changed, 246 insertions(+), 28 deletions(-) create mode 100644 examples/motion_detection.rs diff --git a/examples/motion_detection.rs b/examples/motion_detection.rs new file mode 100644 index 0000000..d619e44 --- /dev/null +++ b/examples/motion_detection.rs @@ -0,0 +1,34 @@ +use mpu6050::*; +use linux_embedded_hal::{I2cdev, Delay}; +use i2cdev::linux::LinuxI2CError; +use embedded_hal::blocking::delay::DelayMs; +use mpu6050::device::MOT_DETECT_STATUS; + +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)?; + mpu.setup_motion_detection(&mut delay)?; + + let mut count: u8 = 0; + + loop { + if mpu.get_motion_detected()? { + println!("YEAH BUDDY. Motion by axes: {:b}", mpu.read_byte(MOT_DETECT_STATUS::ADDR)?); + count += 1; + } + + delay.delay_ms(10u8); + + if count > 5 { + mpu.reset_device(&mut delay)?; + break; + } + } + + Ok(()) +} diff --git a/misc/status.md b/misc/status.md index 6c77e99..f8b9e29 100644 --- a/misc/status.md +++ b/misc/status.md @@ -19,7 +19,7 @@ | ||[0x17] ZG_OFFS_USRH| R/W | [15:0] ZG_OFFS_USR| | ||[0x18] ZG_OFFS_USRL| R/W || | ||[0x19] SMPLRT_DIV| R/W | [7:0] SMPLRT_DIV| -| ||[0x1A] CONFIG| R/W | [5:3] EXT_SYNC_SET [2:0] DLPF_CFG| +| ||[0x1A] CONFIG| R/W | [5:3] EXT_SYNC_SET [2:0] DLPF_CFG| | ||[0x1B] GYRO_CONFIG| R/W | [7] XG_ST [6] YG_ST [5] ZG_ST [4:3] FS_SEL| | ||[0x1C] ACCEL_CONFIG| R/W | [7] XA_ST [6] YA_ST [5] ZA_ST [4:3] AFS_SEL [2:0] ACCEL_HPF| | ||[0x1D] FF_THR| R/W | [7:0] FF_THR| @@ -48,24 +48,24 @@ | ||[0x34] I2C_SLV4_CTRL| R/W | [7] I2C_SLV4_EN [6] I2C_SLV4_INT_EN [5] I2C_SLV4_REG_DIS [4:0] I2C_MST_DLY| | ||[0x35] I2C_SLV4_DI| R/W | [7:0] I2C_SLV4_DI| | ||[0x36] I2C_MST_STATUS| RO| [7] PASS_THROUGH [6] I2C_SLV4_DONE [5] I2C_LOST_ARB [4] I2C_SLV4_NACK [3] I2C_SLV3_NACK [2] I2C_SLV2_NACK [1] I2C_SLV1_NACK [0] I2C_SLV0_NACK| -| ||[0x37] INT_PIN_CFG| R/W | [7] INT_LEVEL [6] INT_OPEN [5] LATCH_INT_EN [4] INT_RD_CLEAR [3] FSYNC_INT_LEVEL [2] FSYNC_INT_EN [1] I2C_BYPASS_EN [0] CLKOUT_EN| -| ||[0x38] INT_ENABLE| R/W | [7] FF_EN [6] MOT_EN [5] ZMOT_EN [4] FIFO_OFLOW_EN [3] I2C_MST_INT_EN [2] PLL_RDY_INT_EN [1] DMP_INT_EN [0] RAW_RDY_EN| +| ||[0x37] INT_PIN_CFG| R/W | [7] INT_LEVEL [6] INT_OPEN [5] LATCH_INT_EN [4] INT_RD_CLEAR [3] FSYNC_INT_LEVEL [2] FSYNC_INT_EN [1] I2C_BYPASS_EN [0] CLKOUT_EN| +| ||[0x38] INT_ENABLE| R/W | [7] FF_EN [6] MOT_EN [5] ZMOT_EN [4] FIFO_OFLOW_EN [3] I2C_MST_INT_EN [2] PLL_RDY_INT_EN [1] DMP_INT_EN [0] RAW_RDY_EN| | ||[0x39] DMP_INT_STATUS| RO| [5] DMP_INT_5 [4] DMP_INT_4 [3] DMP_INT_3 [2] DMP_INT_2 [1] DMP_INT_1 [0] DMP_INT_0| -| ||[0x3A] INT_STATUS| RO| [7] FF_INT [6] MOT_INT [5] ZMOT_INT [4] FIFO_OFLOW_INT [3] I2C_MST_INT [2] PLL_RDY_INT [1] DMP_INT [0] RAW_RDY_INT| -| |[0x3B] ACCEL_XOUT_H| RO| [15:0] ACCEL_XOUT| -| |[0x3C] ACCEL_XOUT_L| RO| -| |[0x3D] ACCEL_YOUT_H| RO| [15:0] ACCEL_YOUT| -| |[0x3E] ACCEL_YOUT_L| RO| -| |[0x3F] ACCEL_ZOUT_H| RO| [15:0] ACCEL_ZOUT| -| |[0x40] ACCEL_ZOUT_L| RO|| -| |[0x41] TEMP_OUT_H| RO| [15:0] TEMP_OUT| -| |[0x42] TEMP_OUT_L| RO|| -| |[0x43] GYRO_XOUT_H| RO | [15:0] GYRO_XOUT| -| |[0x44] GYRO_XOUT_L| RO|| -| |[0x45] GYRO_YOUT_H| RO| [15:0] GYRO_YOUT| -| |[0x46] GYRO_YOUT_L| RO|| -| |[0x47] GYRO_ZOUT_H| RO| [15:0] GYRO_ZOUT| -| |[0x48] GYRO_ZOUT_L| RO|| +| ||[0x3A] INT_STATUS| RO| [7] FF_INT [6] MOT_INT [5] ZMOT_INT [4] FIFO_OFLOW_INT [3] I2C_MST_INT [2] PLL_RDY_INT [1] DMP_INT [0] RAW_RDY_INT| +| ||[0x3B] ACCEL_XOUT_H| RO| [15:0] ACCEL_XOUT| +| ||[0x3C] ACCEL_XOUT_L| RO| +| ||[0x3D] ACCEL_YOUT_H| RO| [15:0] ACCEL_YOUT| +| ||[0x3E] ACCEL_YOUT_L| RO| +| ||[0x3F] ACCEL_ZOUT_H| RO| [15:0] ACCEL_ZOUT| +| ||[0x40] ACCEL_ZOUT_L| RO|| +| ||[0x41] TEMP_OUT_H| RO| [15:0] TEMP_OUT| +| ||[0x42] TEMP_OUT_L| RO|| +| ||[0x43] GYRO_XOUT_H| RO | [15:0] GYRO_XOUT| +| ||[0x44] GYRO_XOUT_L| RO|| +| ||[0x45] GYRO_YOUT_H| RO| [15:0] GYRO_YOUT| +| ||[0x46] GYRO_YOUT_L| RO|| +| ||[0x47] GYRO_ZOUT_H| RO| [15:0] GYRO_ZOUT| +| ||[0x48] GYRO_ZOUT_L| RO|| | ||[0x49] EXT_SENS_DATA_00| RO |[7:0] EXT_SENS_DATA_00| | ||[0x4A] EXT_SENS_DATA_01| RO |[7:0] EXT_SENS_DATA_01| | ||[0x4B] EXT_SENS_DATA_02| RO |[7:0] EXT_SENS_DATA_02| @@ -90,14 +90,14 @@ | ||[0x5E] EXT_SENS_DATA_21| RO |[7:0] EXT_SENS_DATA_21| | ||[0x5F] EXT_SENS_DATA_22| RO |[7:0] EXT_SENS_DATA_22| | ||[0x60] EXT_SENS_DATA_23| RO |[7:0] EXT_SENS_DATA_23| -| ||[0x61] MOT_DETECT_STATUS| RO |[7] MOT_XNEG [6] MOT_XPOS [5] MOT_YNEG [4] MOT_YPOS [3] MOT_ZNEG [2] MOT_ZPOS [0] MOT_ZRMOT| +| ||[0x61] MOT_DETECT_STATUS| RO |[7] MOT_XNEG [6] MOT_XPOS [5] MOT_YNEG [4] MOT_YPOS [3] MOT_ZNEG [2] MOT_ZPOS [0] MOT_ZRMOT| | ||[0x63] I2C_SLV0_DO| R/W | [7:0] I2C_SLV0_DO| | ||[0x64] I2C_SLV1_DO| R/W | [7:0] I2C_SLV1_DO| | ||[0x65] I2C_SLV2_DO| R/W | [7:0] I2C_SLV2_DO| | ||[0x66] I2C_SLV3_DO| R/W | [7:0] I2C_SLV3_DO| | ||[0x67] I2C_MST_DELAY_CTRL| R/W | [7] DELAY_ES_SHADOW [4] I2C_SLV4_DLY_EN [3] I2C_SLV3_DLY_EN [2] I2C_SLV2_DLY_EN [1] I2C_SLV1_DLY_EN [0] I2C_SLV0_DLY_EN| | ||[0x68] SIGNAL_PATH_RESET| R/W | [2] GYRO_RESET [1] ACCEL_RESET [0] TEMP_RESET| -| ||[0x69] MOT_DETECT_CTRL| R/W | [5:4] ACCEL_ON_DELAY [3:2] FF_COUNT [1:0] MOT_COUNT| +| ||[0x69] MOT_DETECT_CTRL| R/W | [5:4] ACCEL_ON_DELAY [3:2] FF_COUNT [1:0] MOT_COUNT| | ||[0x6A] USER_CTRL| R/W | [7] DMP_EN [6] FIFO_EN [5] I2C_MST_EN [4] I2C_IF_DIS [3] DMP_RESET [2] FIFO_RESET [1] I2C_MST_RESET [0] SIG_COND_RESET| | ||[0x6B] PWR_MGMT_1| R/W | [7] DEVICE_RESET [6] SLEEP [5] CYCLE [3] TEMP_DIS [2:0] CLK_SEL| | ||[0x6C] PWR_MGMT_2| R/W | [7] LP_WAKE_CTRL [5] STBY_ZG [4] STBY_YA [3] STBY_ZA [2] STBY_XG [1] STBY_YG [0] STBY_ZG| diff --git a/src/device.rs b/src/device.rs index b5517e3..e67d975 100644 --- a/src/device.rs +++ b/src/device.rs @@ -2,6 +2,7 @@ //! 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 /// @@ -46,10 +47,6 @@ impl Specs { #[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 @@ -64,10 +61,18 @@ pub enum Registers { ACC_REGZ_H = 0x3f, /// High Byte Register Temperature TEMP_OUT_H = 0x41, - /// Internal clock - POWER_MGMT_2 = 0x6c, + // } +/// Slave address of Mpu6050 +pub const SLAVE_ADDR: u8 = 0x68; +/// Internal register to check slave addr +pub const WHOAMI: u8 = 0x75; +/// Motion Threshold Register +pub const MOT_THR: u8 = 0x1F; +/// Motion Duration Detection Register +pub const MOT_DUR: u8 = 0x20; + /// Describes a bit block from bit number 'bit' to 'bit'+'length' pub struct BitBlock { pub bit: u8, @@ -80,6 +85,20 @@ impl Registers { } } +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 26: Configuration (DLPF, External signal) +pub struct CONFIG; + +impl CONFIG { + /// Base Address + pub const ADDR: u8 = 0x1a; + /// external Frame Synchronisation (FSYNC) + pub const EXT_SYNC_SET: BitBlock = BitBlock { bit: 5, length: 3}; + /// Digital Low Pass Filter (DLPF) config + pub const DLPF_CFG: BitBlock = BitBlock { bit: 2, length: 3}; +} + #[allow(non_camel_case_types)] #[derive(Copy, Clone, Debug)] /// Register 27: Gyro Config @@ -119,7 +138,117 @@ impl ACCEL_CONFIG { #[allow(non_camel_case_types)] #[derive(Copy, Clone, Debug)] -/// Register 107: Power Management +/// Register 55: INT Pin / Bypass Enable Configuration +pub struct INT_PIN_CFG; + +impl INT_PIN_CFG { + /// Base Address + pub const ADDR: u8 = 0x37; + /// INT pin logic level + pub const INT_LEVEL: u8 = 7; + /// INT pin config + pub const INT_OPEN: u8 = 6; + /// Pulse (length) + pub const LATCH_INT_EN: u8 = 5; + /// INT clear conditions + pub const INT_RD_CLEAR: u8 = 4; + /// FSYNC PIN logic level + pub const FSYNC_INT_LEVEL: u8 = 3; + /// FSYNC PIN config + pub const FSYNC_INT_EN: u8 = 2; + /// i2c access/bypass + pub const I2C_BYPASS_EN: u8 = 1; + /// enable/disable reference clock output + pub const CLKOUT_EN: u8 = 0; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 56: Interrupt Status +pub struct INT_ENABLE; + +impl INT_ENABLE { + /// Base Address + pub const ADDR: u8 = 0x38; + /// Generate interrupt Free Fall Detection + pub const FF_EN: u8 = 7; + /// Generate interrupt with Motion Detected + pub const MOT_EN: u8 = 6; + /// Generate iterrrupt when Zero Motion Detection + pub const ZMOT_EN: u8 = 5; + /// Generate iterrupt when FIFO buffer overflow + pub const FIFO_OFLOW_END: u8 = 4; + /// this bit enables any of the I2C Masterinterrupt sources to generate an interrupt + pub const I2C_MST_INT_EN: u8 = 3; + /// enables Data Ready interrupt, each time a write operation to all sensor registers completed + pub const DATA_RDY_EN: u8 = 0; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 58: Interrupt Status +pub struct INT_STATUS; + +impl INT_STATUS { + /// Base Address + pub const ADDR: u8 = 0x3a; + /// Free Fall Interrupt + pub const FF_INT: u8 = 7; + /// Motion Detection Interrupt + pub const MOT_INT: u8 = 6; + /// Zero Motion Detection Interrupt + pub const ZMOT_INT: u8 = 5; + /// FIFO buffer overflow + pub const FIFO_OFLOW_INT: u8 = 4; + /// i2c master interrupt has been generated + pub const I2C_MSF_INT: u8 = 3; + /// Data is ready + pub const DATA_RDY_INT: u8 = 0; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 97: Motion Detection Status +pub struct MOT_DETECT_STATUS; + +impl MOT_DETECT_STATUS { + /// Base Address + pub const ADDR: u8 = 0x61; + /// motion in the negative X axis has generated a Motion detection interrupt + pub const MOT_XNEG: u8 = 7; + /// motion in the positive X axis has generated a Motion detection interrupt + pub const MOT_XPOS: u8 = 6; + /// motion in the negative Y axis has generated a Motion detection interrupt + pub const MOT_YNEG: u8 = 5; + /// motion in the positive Y axis has generated a Motion detection interrupt + pub const MOT_YPOS: u8 = 4; + /// motion in the negative Z axis has generated a Motion detection interrupt. + pub const MOT_ZNEG: u8 = 3; + /// motion in the positive Z axis has generated a Motion detection interrupt + pub const MOT_ZPOS: u8 = 2; + /// Zero Motion detection interrupt is generated + pub const MOT_ZRMOT: u8 = 0; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 105: Motion Detection Control +pub struct MOT_DETECT_CONTROL; + +impl MOT_DETECT_CONTROL { + /// Base Address + pub const ADDR: u8 = 0x69; + /// Additional delay + pub const ACCEL_ON_DELAY: BitBlock = BitBlock { bit: 5, length: 2}; + /// Free Fall count + pub const FF_COUNT: BitBlock = BitBlock { bit: 3, length: 2}; + /// Motion Detection cound + pub const MOT_COUNT: BitBlock = BitBlock { bit: 1, length: 2}; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 107: Power Management 1 pub struct PWR_MGMT_1; impl PWR_MGMT_1 { @@ -137,6 +266,40 @@ impl PWR_MGMT_1 { pub const CLKSEL: BitBlock = BitBlock { bit: 2, length: 3 }; } +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 107: Power Management 2 +pub struct PWR_MGMT_2; + +impl PWR_MGMT_2 { + /// Base Address + pub const ADDR: u8 = 0x6c; + /// Wake up frequency + pub const LP_WAKE_CTRL: BitBlock = BitBlock { bit: 7, length: 2}; + /// disable accel axis x + pub const STBY_XA: u8 = 5; + /// disable accel axis y + pub const STBY_YA: u8 = 4; + /// disable accel axis z + pub const STBY_ZA: u8 = 3; + /// disable gyro axis x + pub const STBY_XG: u8 = 2; + /// disable gyro axis y + pub const STBY_YG: u8 = 1; + /// disable gyro axis z + pub const STBY_ZG: u8 = 0; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug, Eq, PartialEq)] +/// Wake values +pub enum LP_WAKE_CTRL { + _1P25 = 0, + _2P5, + _5, + _10, +} + #[allow(non_camel_case_types)] #[derive(Copy, Clone, Debug, Eq, PartialEq)] /// Accelerometer High Pass Filter Values diff --git a/src/lib.rs b/src/lib.rs index 06a2657..b43c512 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -140,13 +140,34 @@ where /// Verifies device to address 0x68 with WHOAMI.addr() Register fn verify(&mut self) -> Result<(), Mpu6050Error> { - let address = self.read_byte(WHOAMI.addr())?; + let address = self.read_byte(WHOAMI)?; if address != SLAVE_ADDR.addr() { return Err(Mpu6050Error::InvalidChipId(address)); } Ok(()) } + /// setup motion detection + /// sources: + /// * https://github.com/kriswiner/MPU6050/blob/a7e0c8ba61a56c5326b2bcd64bc81ab72ee4616b/MPU6050IMU.ino#L486 + /// * https://arduino.stackexchange.com/a/48430 + pub fn setup_motion_detection>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error> { + self.write_byte(0x6B, 0x00)?; + // optional? self.write_byte(0x68, 0x07)?; // Reset all internal signal paths in the MPU-6050 by writing 0x07 to register 0x68; + self.write_byte(INT_PIN_CFG::ADDR, 0x20)?; //write register 0x37 to select how to use the interrupt pin. For an active high, push-pull signal that stays until register (decimal) 58 is read, write 0x20. + self.write_byte(ACCEL_CONFIG::ADDR, 0x01)?; //Write register 28 (==0x1C) to set the Digital High Pass Filter, bits 3:0. For example set it to 0x01 for 5Hz. (These 3 bits are grey in the data sheet, but they are used! Leaving them 0 means the filter always outputs 0.) + self.write_byte(MOT_THR, 10)?; //Write the desired Motion threshold to register 0x1F (For example, write decimal 20). + self.write_byte(MOT_DUR, 40)?; //Set motion detect duration to 1 ms; LSB is 1 ms @ 1 kHz rate + self.write_byte(0x69, 0x15)?; //to register 0x69, write the motion detection decrement and a few other settings (for example write 0x15 to set both free-fall and motion decrements to 1 and accelerometer start-up delay to 5ms total by adding 1ms. ) + self.write_byte(INT_ENABLE::ADDR, 0x40)?; //write register 0x38, bit 6 (0x40), to enable motion detection interrupt. + Ok(()) + } + + /// get whether or not motion has been detected (INT_STATUS, MOT_INT) + pub fn get_motion_detected(&mut self) -> Result> { + Ok(self.read_bit(INT_STATUS::ADDR, INT_STATUS::MOT_INT)? != 0) + } + /// set accel high pass filter mode pub fn set_accel_hpf(&mut self, mode: ACCEL_HPF) -> Result<(), Mpu6050Error> { Ok( From 597d81dfaada1d4269740724bd044b0f8db006fd Mon Sep 17 00:00:00 2001 From: juliangaal Date: Fri, 19 Feb 2021 22:57:04 +0100 Subject: [PATCH 2/4] enable motion detection --- examples/motion_detection.rs | 34 +++++++ misc/status.md | 40 ++++---- src/device.rs | 177 +++++++++++++++++++++++++++++++++-- src/lib.rs | 25 ++++- 4 files changed, 247 insertions(+), 29 deletions(-) create mode 100644 examples/motion_detection.rs diff --git a/examples/motion_detection.rs b/examples/motion_detection.rs new file mode 100644 index 0000000..d619e44 --- /dev/null +++ b/examples/motion_detection.rs @@ -0,0 +1,34 @@ +use mpu6050::*; +use linux_embedded_hal::{I2cdev, Delay}; +use i2cdev::linux::LinuxI2CError; +use embedded_hal::blocking::delay::DelayMs; +use mpu6050::device::MOT_DETECT_STATUS; + +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)?; + mpu.setup_motion_detection(&mut delay)?; + + let mut count: u8 = 0; + + loop { + if mpu.get_motion_detected()? { + println!("YEAH BUDDY. Motion by axes: {:b}", mpu.read_byte(MOT_DETECT_STATUS::ADDR)?); + count += 1; + } + + delay.delay_ms(10u8); + + if count > 5 { + mpu.reset_device(&mut delay)?; + break; + } + } + + Ok(()) +} diff --git a/misc/status.md b/misc/status.md index 6c77e99..f8b9e29 100644 --- a/misc/status.md +++ b/misc/status.md @@ -19,7 +19,7 @@ |
  • -[ ]
|
  • -[ ]
|[0x17] ZG_OFFS_USRH| R/W | [15:0] ZG_OFFS_USR| |
  • -[ ]
|
  • -[ ]
|[0x18] ZG_OFFS_USRL| R/W || |
  • -[ ]
|
  • -[ ]
|[0x19] SMPLRT_DIV| R/W | [7:0] SMPLRT_DIV| -|
  • -[ ]
|
  • -[ ]
|[0x1A] CONFIG| R/W | [5:3] EXT_SYNC_SET [2:0] DLPF_CFG| +|
  • -[x]
|
  • -[ ]
|[0x1A] CONFIG| R/W | [5:3] EXT_SYNC_SET [2:0] DLPF_CFG| |
  • -[x]
|
  • -[x]
|[0x1B] GYRO_CONFIG| R/W | [7] XG_ST [6] YG_ST [5] ZG_ST [4:3] FS_SEL| |
  • -[x]
|
  • -[x]
|[0x1C] ACCEL_CONFIG| R/W | [7] XA_ST [6] YA_ST [5] ZA_ST [4:3] AFS_SEL [2:0] ACCEL_HPF| |
  • -[ ]
|
  • -[ ]
|[0x1D] FF_THR| R/W | [7:0] FF_THR| @@ -48,24 +48,24 @@ |
  • -[ ]
|
  • -[ ]
|[0x34] I2C_SLV4_CTRL| R/W | [7] I2C_SLV4_EN [6] I2C_SLV4_INT_EN [5] I2C_SLV4_REG_DIS [4:0] I2C_MST_DLY| |
  • -[ ]
|
  • -[ ]
|[0x35] I2C_SLV4_DI| R/W | [7:0] I2C_SLV4_DI| |
  • -[ ]
|
  • -[ ]
|[0x36] I2C_MST_STATUS| RO| [7] PASS_THROUGH [6] I2C_SLV4_DONE [5] I2C_LOST_ARB [4] I2C_SLV4_NACK [3] I2C_SLV3_NACK [2] I2C_SLV2_NACK [1] I2C_SLV1_NACK [0] I2C_SLV0_NACK| -|
  • -[ ]
|
  • -[ ]
|[0x37] INT_PIN_CFG| R/W | [7] INT_LEVEL [6] INT_OPEN [5] LATCH_INT_EN [4] INT_RD_CLEAR [3] FSYNC_INT_LEVEL [2] FSYNC_INT_EN [1] I2C_BYPASS_EN [0] CLKOUT_EN| -|
  • -[ ]
|
  • -[ ]
|[0x38] INT_ENABLE| R/W | [7] FF_EN [6] MOT_EN [5] ZMOT_EN [4] FIFO_OFLOW_EN [3] I2C_MST_INT_EN [2] PLL_RDY_INT_EN [1] DMP_INT_EN [0] RAW_RDY_EN| +|
  • -[x]
|
  • -[ ]
|[0x37] INT_PIN_CFG| R/W | [7] INT_LEVEL [6] INT_OPEN [5] LATCH_INT_EN [4] INT_RD_CLEAR [3] FSYNC_INT_LEVEL [2] FSYNC_INT_EN [1] I2C_BYPASS_EN [0] CLKOUT_EN| +|
  • -[x]
|
  • -[ ]
|[0x38] INT_ENABLE| R/W | [7] FF_EN [6] MOT_EN [5] ZMOT_EN [4] FIFO_OFLOW_EN [3] I2C_MST_INT_EN [2] PLL_RDY_INT_EN [1] DMP_INT_EN [0] RAW_RDY_EN| |
  • -[ ]
|
  • -[ ]
|[0x39] DMP_INT_STATUS| RO| [5] DMP_INT_5 [4] DMP_INT_4 [3] DMP_INT_3 [2] DMP_INT_2 [1] DMP_INT_1 [0] DMP_INT_0| -|
  • -[ ]
|
  • -[ ]
|[0x3A] INT_STATUS| RO| [7] FF_INT [6] MOT_INT [5] ZMOT_INT [4] FIFO_OFLOW_INT [3] I2C_MST_INT [2] PLL_RDY_INT [1] DMP_INT [0] RAW_RDY_INT| -|
  • -[x]
  • -[x]
|[0x3B] ACCEL_XOUT_H| RO| [15:0] ACCEL_XOUT| -|
  • -[x]
  • -[x]
|[0x3C] ACCEL_XOUT_L| RO| -|
  • -[x]
  • -[x]
|[0x3D] ACCEL_YOUT_H| RO| [15:0] ACCEL_YOUT| -|
  • -[x]
  • -[x]
|[0x3E] ACCEL_YOUT_L| RO| -|
  • -[x]
  • -[x]
|[0x3F] ACCEL_ZOUT_H| RO| [15:0] ACCEL_ZOUT| -|
  • -[x]
  • -[x]
|[0x40] ACCEL_ZOUT_L| RO|| -|
  • -[x]
  • -[x]
|[0x41] TEMP_OUT_H| RO| [15:0] TEMP_OUT| -|
  • -[x]
  • -[x]
|[0x42] TEMP_OUT_L| RO|| -|
  • -[x]
  • -[x]
|[0x43] GYRO_XOUT_H| RO | [15:0] GYRO_XOUT| -|
  • -[x]
  • -[x]
|[0x44] GYRO_XOUT_L| RO|| -|
  • -[x]
  • -[x]
|[0x45] GYRO_YOUT_H| RO| [15:0] GYRO_YOUT| -|
  • -[x]
  • -[x]
|[0x46] GYRO_YOUT_L| RO|| -|
  • -[x]
  • -[x]
|[0x47] GYRO_ZOUT_H| RO| [15:0] GYRO_ZOUT| -|
  • -[x]
  • -[x]
|[0x48] GYRO_ZOUT_L| RO|| +|
  • -[x]
|
  • -[ ]
|[0x3A] INT_STATUS| RO| [7] FF_INT [6] MOT_INT [5] ZMOT_INT [4] FIFO_OFLOW_INT [3] I2C_MST_INT [2] PLL_RDY_INT [1] DMP_INT [0] RAW_RDY_INT| +|
  • -[x]
|
  • -[x]
|[0x3B] ACCEL_XOUT_H| RO| [15:0] ACCEL_XOUT| +|
  • -[x]
|
  • -[x]
|[0x3C] ACCEL_XOUT_L| RO| +|
  • -[x]
|
  • -[x]
|[0x3D] ACCEL_YOUT_H| RO| [15:0] ACCEL_YOUT| +|
  • -[x]
|
  • -[x]
|[0x3E] ACCEL_YOUT_L| RO| +|
  • -[x]
|
  • -[x]
|[0x3F] ACCEL_ZOUT_H| RO| [15:0] ACCEL_ZOUT| +|
  • -[x]
|
  • -[x]
|[0x40] ACCEL_ZOUT_L| RO|| +|
  • -[x]
|
  • -[x]
|[0x41] TEMP_OUT_H| RO| [15:0] TEMP_OUT| +|
  • -[x]
|
  • -[x]
|[0x42] TEMP_OUT_L| RO|| +|
  • -[x]
|
  • -[x]
|[0x43] GYRO_XOUT_H| RO | [15:0] GYRO_XOUT| +|
  • -[x]
|
  • -[x]
|[0x44] GYRO_XOUT_L| RO|| +|
  • -[x]
|
  • -[x]
|[0x45] GYRO_YOUT_H| RO| [15:0] GYRO_YOUT| +|
  • -[x]
|
  • -[x]
|[0x46] GYRO_YOUT_L| RO|| +|
  • -[x]
|
  • -[x]
|[0x47] GYRO_ZOUT_H| RO| [15:0] GYRO_ZOUT| +|
  • -[x]
|
  • -[x]
|[0x48] GYRO_ZOUT_L| RO|| |
  • -[ ]
|
  • -[ ]
|[0x49] EXT_SENS_DATA_00| RO |[7:0] EXT_SENS_DATA_00| |
  • -[ ]
|
  • -[ ]
|[0x4A] EXT_SENS_DATA_01| RO |[7:0] EXT_SENS_DATA_01| |
  • -[ ]
|
  • -[ ]
|[0x4B] EXT_SENS_DATA_02| RO |[7:0] EXT_SENS_DATA_02| @@ -90,14 +90,14 @@ |
  • -[ ]
|
  • -[ ]
|[0x5E] EXT_SENS_DATA_21| RO |[7:0] EXT_SENS_DATA_21| |
  • -[ ]
|
  • -[ ]
|[0x5F] EXT_SENS_DATA_22| RO |[7:0] EXT_SENS_DATA_22| |
  • -[ ]
|
  • -[ ]
|[0x60] EXT_SENS_DATA_23| RO |[7:0] EXT_SENS_DATA_23| -|
  • -[ ]
|
  • -[ ]
|[0x61] MOT_DETECT_STATUS| RO |[7] MOT_XNEG [6] MOT_XPOS [5] MOT_YNEG [4] MOT_YPOS [3] MOT_ZNEG [2] MOT_ZPOS [0] MOT_ZRMOT| +|
  • -[x]
|
  • -[ ]
|[0x61] MOT_DETECT_STATUS| RO |[7] MOT_XNEG [6] MOT_XPOS [5] MOT_YNEG [4] MOT_YPOS [3] MOT_ZNEG [2] MOT_ZPOS [0] MOT_ZRMOT| |
  • -[ ]
|
  • -[ ]
|[0x63] I2C_SLV0_DO| R/W | [7:0] I2C_SLV0_DO| |
  • -[ ]
|
  • -[ ]
|[0x64] I2C_SLV1_DO| R/W | [7:0] I2C_SLV1_DO| |
  • -[ ]
|
  • -[ ]
|[0x65] I2C_SLV2_DO| R/W | [7:0] I2C_SLV2_DO| |
  • -[ ]
|
  • -[ ]
|[0x66] I2C_SLV3_DO| R/W | [7:0] I2C_SLV3_DO| |
  • -[ ]
|
  • -[ ]
|[0x67] I2C_MST_DELAY_CTRL| R/W | [7] DELAY_ES_SHADOW [4] I2C_SLV4_DLY_EN [3] I2C_SLV3_DLY_EN [2] I2C_SLV2_DLY_EN [1] I2C_SLV1_DLY_EN [0] I2C_SLV0_DLY_EN| |
  • -[ ]
|
  • -[ ]
|[0x68] SIGNAL_PATH_RESET| R/W | [2] GYRO_RESET [1] ACCEL_RESET [0] TEMP_RESET| -|
  • -[ ]
|
  • -[ ]
|[0x69] MOT_DETECT_CTRL| R/W | [5:4] ACCEL_ON_DELAY [3:2] FF_COUNT [1:0] MOT_COUNT| +|
  • -[x]
|
  • -[ ]
|[0x69] MOT_DETECT_CTRL| R/W | [5:4] ACCEL_ON_DELAY [3:2] FF_COUNT [1:0] MOT_COUNT| |
  • -[ ]
|
  • -[ ]
|[0x6A] USER_CTRL| R/W | [7] DMP_EN [6] FIFO_EN [5] I2C_MST_EN [4] I2C_IF_DIS [3] DMP_RESET [2] FIFO_RESET [1] I2C_MST_RESET [0] SIG_COND_RESET| |
  • -[x]
|
  • -[x]
|[0x6B] PWR_MGMT_1| R/W | [7] DEVICE_RESET [6] SLEEP [5] CYCLE [3] TEMP_DIS [2:0] CLK_SEL| |
  • -[x]
|
  • -[ ]
|[0x6C] PWR_MGMT_2| R/W | [7] LP_WAKE_CTRL [5] STBY_ZG [4] STBY_YA [3] STBY_ZA [2] STBY_XG [1] STBY_YG [0] STBY_ZG| diff --git a/src/device.rs b/src/device.rs index b5517e3..e67d975 100644 --- a/src/device.rs +++ b/src/device.rs @@ -2,6 +2,7 @@ //! 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 /// @@ -46,10 +47,6 @@ impl Specs { #[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 @@ -64,10 +61,18 @@ pub enum Registers { ACC_REGZ_H = 0x3f, /// High Byte Register Temperature TEMP_OUT_H = 0x41, - /// Internal clock - POWER_MGMT_2 = 0x6c, + // } +/// Slave address of Mpu6050 +pub const SLAVE_ADDR: u8 = 0x68; +/// Internal register to check slave addr +pub const WHOAMI: u8 = 0x75; +/// Motion Threshold Register +pub const MOT_THR: u8 = 0x1F; +/// Motion Duration Detection Register +pub const MOT_DUR: u8 = 0x20; + /// Describes a bit block from bit number 'bit' to 'bit'+'length' pub struct BitBlock { pub bit: u8, @@ -80,6 +85,20 @@ impl Registers { } } +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 26: Configuration (DLPF, External signal) +pub struct CONFIG; + +impl CONFIG { + /// Base Address + pub const ADDR: u8 = 0x1a; + /// external Frame Synchronisation (FSYNC) + pub const EXT_SYNC_SET: BitBlock = BitBlock { bit: 5, length: 3}; + /// Digital Low Pass Filter (DLPF) config + pub const DLPF_CFG: BitBlock = BitBlock { bit: 2, length: 3}; +} + #[allow(non_camel_case_types)] #[derive(Copy, Clone, Debug)] /// Register 27: Gyro Config @@ -119,7 +138,117 @@ impl ACCEL_CONFIG { #[allow(non_camel_case_types)] #[derive(Copy, Clone, Debug)] -/// Register 107: Power Management +/// Register 55: INT Pin / Bypass Enable Configuration +pub struct INT_PIN_CFG; + +impl INT_PIN_CFG { + /// Base Address + pub const ADDR: u8 = 0x37; + /// INT pin logic level + pub const INT_LEVEL: u8 = 7; + /// INT pin config + pub const INT_OPEN: u8 = 6; + /// Pulse (length) + pub const LATCH_INT_EN: u8 = 5; + /// INT clear conditions + pub const INT_RD_CLEAR: u8 = 4; + /// FSYNC PIN logic level + pub const FSYNC_INT_LEVEL: u8 = 3; + /// FSYNC PIN config + pub const FSYNC_INT_EN: u8 = 2; + /// i2c access/bypass + pub const I2C_BYPASS_EN: u8 = 1; + /// enable/disable reference clock output + pub const CLKOUT_EN: u8 = 0; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 56: Interrupt Status +pub struct INT_ENABLE; + +impl INT_ENABLE { + /// Base Address + pub const ADDR: u8 = 0x38; + /// Generate interrupt Free Fall Detection + pub const FF_EN: u8 = 7; + /// Generate interrupt with Motion Detected + pub const MOT_EN: u8 = 6; + /// Generate iterrrupt when Zero Motion Detection + pub const ZMOT_EN: u8 = 5; + /// Generate iterrupt when FIFO buffer overflow + pub const FIFO_OFLOW_END: u8 = 4; + /// this bit enables any of the I2C Masterinterrupt sources to generate an interrupt + pub const I2C_MST_INT_EN: u8 = 3; + /// enables Data Ready interrupt, each time a write operation to all sensor registers completed + pub const DATA_RDY_EN: u8 = 0; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 58: Interrupt Status +pub struct INT_STATUS; + +impl INT_STATUS { + /// Base Address + pub const ADDR: u8 = 0x3a; + /// Free Fall Interrupt + pub const FF_INT: u8 = 7; + /// Motion Detection Interrupt + pub const MOT_INT: u8 = 6; + /// Zero Motion Detection Interrupt + pub const ZMOT_INT: u8 = 5; + /// FIFO buffer overflow + pub const FIFO_OFLOW_INT: u8 = 4; + /// i2c master interrupt has been generated + pub const I2C_MSF_INT: u8 = 3; + /// Data is ready + pub const DATA_RDY_INT: u8 = 0; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 97: Motion Detection Status +pub struct MOT_DETECT_STATUS; + +impl MOT_DETECT_STATUS { + /// Base Address + pub const ADDR: u8 = 0x61; + /// motion in the negative X axis has generated a Motion detection interrupt + pub const MOT_XNEG: u8 = 7; + /// motion in the positive X axis has generated a Motion detection interrupt + pub const MOT_XPOS: u8 = 6; + /// motion in the negative Y axis has generated a Motion detection interrupt + pub const MOT_YNEG: u8 = 5; + /// motion in the positive Y axis has generated a Motion detection interrupt + pub const MOT_YPOS: u8 = 4; + /// motion in the negative Z axis has generated a Motion detection interrupt. + pub const MOT_ZNEG: u8 = 3; + /// motion in the positive Z axis has generated a Motion detection interrupt + pub const MOT_ZPOS: u8 = 2; + /// Zero Motion detection interrupt is generated + pub const MOT_ZRMOT: u8 = 0; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 105: Motion Detection Control +pub struct MOT_DETECT_CONTROL; + +impl MOT_DETECT_CONTROL { + /// Base Address + pub const ADDR: u8 = 0x69; + /// Additional delay + pub const ACCEL_ON_DELAY: BitBlock = BitBlock { bit: 5, length: 2}; + /// Free Fall count + pub const FF_COUNT: BitBlock = BitBlock { bit: 3, length: 2}; + /// Motion Detection cound + pub const MOT_COUNT: BitBlock = BitBlock { bit: 1, length: 2}; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 107: Power Management 1 pub struct PWR_MGMT_1; impl PWR_MGMT_1 { @@ -137,6 +266,40 @@ impl PWR_MGMT_1 { pub const CLKSEL: BitBlock = BitBlock { bit: 2, length: 3 }; } +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug)] +/// Register 107: Power Management 2 +pub struct PWR_MGMT_2; + +impl PWR_MGMT_2 { + /// Base Address + pub const ADDR: u8 = 0x6c; + /// Wake up frequency + pub const LP_WAKE_CTRL: BitBlock = BitBlock { bit: 7, length: 2}; + /// disable accel axis x + pub const STBY_XA: u8 = 5; + /// disable accel axis y + pub const STBY_YA: u8 = 4; + /// disable accel axis z + pub const STBY_ZA: u8 = 3; + /// disable gyro axis x + pub const STBY_XG: u8 = 2; + /// disable gyro axis y + pub const STBY_YG: u8 = 1; + /// disable gyro axis z + pub const STBY_ZG: u8 = 0; +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, Debug, Eq, PartialEq)] +/// Wake values +pub enum LP_WAKE_CTRL { + _1P25 = 0, + _2P5, + _5, + _10, +} + #[allow(non_camel_case_types)] #[derive(Copy, Clone, Debug, Eq, PartialEq)] /// Accelerometer High Pass Filter Values diff --git a/src/lib.rs b/src/lib.rs index 06a2657..29856fc 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -140,13 +140,34 @@ where /// Verifies device to address 0x68 with WHOAMI.addr() Register fn verify(&mut self) -> Result<(), Mpu6050Error> { - let address = self.read_byte(WHOAMI.addr())?; - if address != SLAVE_ADDR.addr() { + let address = self.read_byte(WHOAMI)?; + if address != SLAVE_ADDR { return Err(Mpu6050Error::InvalidChipId(address)); } Ok(()) } + /// setup motion detection + /// sources: + /// * https://github.com/kriswiner/MPU6050/blob/a7e0c8ba61a56c5326b2bcd64bc81ab72ee4616b/MPU6050IMU.ino#L486 + /// * https://arduino.stackexchange.com/a/48430 + pub fn setup_motion_detection>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error> { + self.write_byte(0x6B, 0x00)?; + // optional? self.write_byte(0x68, 0x07)?; // Reset all internal signal paths in the MPU-6050 by writing 0x07 to register 0x68; + self.write_byte(INT_PIN_CFG::ADDR, 0x20)?; //write register 0x37 to select how to use the interrupt pin. For an active high, push-pull signal that stays until register (decimal) 58 is read, write 0x20. + self.write_byte(ACCEL_CONFIG::ADDR, 0x01)?; //Write register 28 (==0x1C) to set the Digital High Pass Filter, bits 3:0. For example set it to 0x01 for 5Hz. (These 3 bits are grey in the data sheet, but they are used! Leaving them 0 means the filter always outputs 0.) + self.write_byte(MOT_THR, 10)?; //Write the desired Motion threshold to register 0x1F (For example, write decimal 20). + self.write_byte(MOT_DUR, 40)?; //Set motion detect duration to 1 ms; LSB is 1 ms @ 1 kHz rate + self.write_byte(0x69, 0x15)?; //to register 0x69, write the motion detection decrement and a few other settings (for example write 0x15 to set both free-fall and motion decrements to 1 and accelerometer start-up delay to 5ms total by adding 1ms. ) + self.write_byte(INT_ENABLE::ADDR, 0x40)?; //write register 0x38, bit 6 (0x40), to enable motion detection interrupt. + Ok(()) + } + + /// get whether or not motion has been detected (INT_STATUS, MOT_INT) + pub fn get_motion_detected(&mut self) -> Result> { + Ok(self.read_bit(INT_STATUS::ADDR, INT_STATUS::MOT_INT)? != 0) + } + /// set accel high pass filter mode pub fn set_accel_hpf(&mut self, mode: ACCEL_HPF) -> Result<(), Mpu6050Error> { Ok( From d830eebbc63a01e33da41a78392689d69ed9a1e8 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Fri, 19 Feb 2021 23:03:13 +0100 Subject: [PATCH 3/4] remove delay when enabling motion detection --- examples/motion_detection.rs | 2 +- src/lib.rs | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/motion_detection.rs b/examples/motion_detection.rs index d619e44..d1569fb 100644 --- a/examples/motion_detection.rs +++ b/examples/motion_detection.rs @@ -12,7 +12,7 @@ fn main() -> Result<(), Mpu6050Error> { let mut mpu = Mpu6050::new(i2c); mpu.init(&mut delay)?; - mpu.setup_motion_detection(&mut delay)?; + mpu.setup_motion_detection()?; let mut count: u8 = 0; diff --git a/src/lib.rs b/src/lib.rs index 29856fc..b1dcf21 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -151,7 +151,7 @@ where /// sources: /// * https://github.com/kriswiner/MPU6050/blob/a7e0c8ba61a56c5326b2bcd64bc81ab72ee4616b/MPU6050IMU.ino#L486 /// * https://arduino.stackexchange.com/a/48430 - pub fn setup_motion_detection>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error> { + pub fn setup_motion_detection(&mut self) -> Result<(), Mpu6050Error> { self.write_byte(0x6B, 0x00)?; // optional? self.write_byte(0x68, 0x07)?; // Reset all internal signal paths in the MPU-6050 by writing 0x07 to register 0x68; self.write_byte(INT_PIN_CFG::ADDR, 0x20)?; //write register 0x37 to select how to use the interrupt pin. For an active high, push-pull signal that stays until register (decimal) 58 is read, write 0x20. @@ -356,7 +356,7 @@ where /// Writes byte to register pub fn write_byte(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6050Error> { - self.i2c.write(SLAVE_ADDR.addr(), &[reg, byte]) + self.i2c.write(SLAVE_ADDR, &[reg, byte]) .map_err(Mpu6050Error::I2c)?; // delay disabled for dev build // TODO: check effects with physical unit @@ -397,14 +397,14 @@ where /// Reads byte from register 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) + self.i2c.write_read(SLAVE_ADDR, &[reg], &mut byte) .map_err(Mpu6050Error::I2c)?; Ok(byte[0]) } /// Reads series of bytes into buf from specified reg pub fn read_bytes(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), Mpu6050Error> { - self.i2c.write_read(SLAVE_ADDR.addr(), &[reg], buf) + self.i2c.write_read(SLAVE_ADDR, &[reg], buf) .map_err(Mpu6050Error::I2c)?; Ok(()) } From fc2a503f0584fb672407d113d744650293f093b2 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Sat, 20 Feb 2021 13:13:04 +0100 Subject: [PATCH 4/4] prep for crate version 1.4 --- README.md | 91 +++---------------- examples/motion_detection.rs | 3 +- examples/simple.rs | 2 +- examples/test.rs | 18 ++-- src/device.rs | 169 +++++++++++++++++++++++++---------- src/lib.rs | 106 +++++++++++----------- src/range.rs | 67 -------------- 7 files changed, 199 insertions(+), 257 deletions(-) diff --git a/README.md b/README.md index e3b5da7..4c6af89 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,17 @@ # `mpu6050` ![crates.io](https://img.shields.io/crates/v/mpu6050.svg) ![CircleCI](https://img.shields.io/circleci/build/github/juliangaal/mpu6050.svg) > no_std driver for the MPU6050 6-axis IMU -[Datasheet](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6500-Datasheet2.pdf) | [Register Map Sheet](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf) +## What Works +* Reading the accelerometer, gyroscope, temperature sensor + * raw + * scaled + * roll/pitch estimation +* Motion Detection +* Setting Accel/Gyro Ranges/Sensitivity +* Setting Accel HPF/LPF -Visualization options for testing and development in [viz branch](https://github.com/juliangaal/mpu6050/tree/viz/viz) - -### Basic usage +## Basic usage +To use this driver you must provide a concrete `embedded_hal` implementation. Here's a [`linux_embedded_hal`](https://github.com/rust-embedded/linux-embedded-hal) example ```rust use mpu6050::*; @@ -39,79 +45,4 @@ fn main() -> Result<(), Mpu6050Error> { println!("acc: {:?}", acc); } } -``` - -### Linux example (Raspberry Pi 3B) - -#### Cross compile -Requirements: -```bash -$ apt-get install -qq gcc-arm-linux-gnueabihf libc6-armhf-cross libc6-dev-armhf-cross -``` - -Rustup: -```bash -$ rustup target add armv7-unknown-linux-gnueabihf -``` -To configure the linker use `.cargo/config` file - -cross-compile with -```bash -$ cargo build --examples --target=armv7-unknown-linux-gnueabihf -``` -Copy binary to rpi, once steps below are successfully completed. - -#### On RPi (RPi OS lite, 15.02.21) - -##### Install i2c dependencies -```bash -$ sudo apt-get install i2c-tools libi2c-dev python-smbus -y -``` - -##### Enable and load kernel modules at boot -1. edit `/etc/modules` and add - ``` - i2c-dev - i2c-bcm2708 - ``` -2. edit `/boot/config.txt` and add/uncomment - ``` - dtparam=i2c_arm=on - dtparam=i2c1=on - ``` -3. reboot -4. check if working with `sudo i2cdetect -y 1` or `sudo i2cdetect -y 0`. You should bet a signal similar to this - ``` - 0 1 2 3 4 5 6 7 8 9 a b c d e f - 00: -- -- -- -- -- -- -- -- -- -- -- -- -- - 10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- - 20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- - 30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- - 40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- - 50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- - 60: -- -- -- -- -- -- -- -- -- -- -- 68 -- -- -- -- - 70: -- -- -- -- -- -- -- -- - ``` -5. Wire up - - | [RPi GPIO](https://www.raspberrypi.org/documentation/usage/gpio/) | MPU6050 | - |:---|---:| - | 5V Power | VCC | - | Ground | GND | - | GPIO 2 | SCL | - | GPIO 3 | SDA | - -## TODO -- [x] init with default settings -- [ ] init with custom settings - - [x] custom sensitivity - - [ ] custom device initialization -- [x] device verification -- [x] basic feature support as described [here](https://github.com/Tijndagamer/mpu6050/blob/master/mpu6050/mpu6050.py) -- [x] rename constants to better match datasheet -- [ ] complementary filter for roll, pitch estimate, possible on device? -- [ ] low pass filter registers? How to use? -- [ ] sample rate devider with register 25? or timer/clock control with PWR_MGMT_2 - - [ ] internal clock, register 108 `POWER_MGMT_2`, [will cycle between sleep mode and waking up to take a single sample of data from active sensors at a rate determined by LP_WAKE_CTRL](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf) (page 41-43) -- [x] plotting [csv data](https://plot.ly/python/plot-data-from-csv/)for testing? -> See viz branch - +``` \ No newline at end of file diff --git a/examples/motion_detection.rs b/examples/motion_detection.rs index d1569fb..25e9299 100644 --- a/examples/motion_detection.rs +++ b/examples/motion_detection.rs @@ -1,8 +1,7 @@ -use mpu6050::*; +use mpu6050::{*, device::MOT_DETECT_STATUS}; use linux_embedded_hal::{I2cdev, Delay}; use i2cdev::linux::LinuxI2CError; use embedded_hal::blocking::delay::DelayMs; -use mpu6050::device::MOT_DETECT_STATUS; fn main() -> Result<(), Mpu6050Error> { let i2c = I2cdev::new("/dev/i2c-1") diff --git a/examples/simple.rs b/examples/simple.rs index b38d143..9a78468 100644 --- a/examples/simple.rs +++ b/examples/simple.rs @@ -16,7 +16,7 @@ fn main() -> Result<(), Mpu6050Error> { let acc = mpu.get_acc_angles()?; println!("r/p: {:?}", acc); - // get temp + // get sensor temp let temp = mpu.get_temp()?; println!("temp: {:?}c", temp); diff --git a/examples/test.rs b/examples/test.rs index 22ae2d4..4be8015 100644 --- a/examples/test.rs +++ b/examples/test.rs @@ -1,4 +1,4 @@ -use mpu6050::*; +use mpu6050::{*, device::*}; use linux_embedded_hal::{I2cdev, Delay}; use i2cdev::linux::LinuxI2CError; @@ -18,15 +18,15 @@ fn main() -> Result<(), Mpu6050Error> { // Test gyro config println!("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); + assert_eq!(mpu.get_gyro_range()?, GyroRange::D250); + mpu.set_gyro_range(GyroRange::D500)?; + assert_eq!(mpu.get_gyro_range()?, GyroRange::D500); // Test accel config println!("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); + assert_eq!(mpu.get_accel_range()?, AccelRange::G2); + mpu.set_accel_range(AccelRange::G4)?; + assert_eq!(mpu.get_accel_range()?, AccelRange::G4); // accel_hpf: per default RESET/no filter, see ACCEL_CONFIG println!("Test accel hpf"); @@ -81,8 +81,8 @@ fn main() -> Result<(), Mpu6050Error> { println!("Test reset"); mpu.reset_device(&mut delay)?; assert_eq!(mpu.get_accel_hpf()?, ACCEL_HPF::_RESET); - assert_eq!(mpu.get_accel_range()?, range::AccelRange::G2); - assert_eq!(mpu.get_gyro_range()?, range::GyroRange::D250); + assert_eq!(mpu.get_accel_range()?, AccelRange::G2); + assert_eq!(mpu.get_gyro_range()?, GyroRange::D250); assert_eq!(mpu.get_sleep_enabled()?, true); assert_eq!(mpu.get_temp_enabled()?, true); diff --git a/src/device.rs b/src/device.rs index e67d975..545ac6e 100644 --- a/src/device.rs +++ b/src/device.rs @@ -1,13 +1,18 @@ -//! 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 +//! All device constants used in the driver, mostly register addresses. //! +//! NOTE: Earlier revisions of Datasheet and Register Map has way more info about interrupt usage, +//! particularly rev 3.2 //! +//! #### Sources: +//! * Register map (rev 3.2): https://arduino.ua/docs/RM-MPU-6000A.pdf +//! * Datasheet (rev 3.2): https://www.cdiweb.com/datasheets/invensense/ps-mpu-6000a.pdf + /// Gyro Sensitivity /// /// Measurements are scaled like this: /// x * range/2**(resolution-1) or x / (2**(resolution-1) / range) +/// /// Sources: /// * https://www.nxp.com/docs/en/application-note/AN3461.pdf /// * https://theccontinuum.com/2012/09/24/arduino-imu-pitch-roll-from-accelerometer/ @@ -19,6 +24,7 @@ pub const GYRO_SENS: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4); /// Accelerometer Sensitivity /// /// Measurements are scaled like this: +/// /// x * range/2**(resolution-1) or x / (2**(resolution-1) / range) /// Sources: /// * https://www.nxp.com/docs/en/application-note/AN3461.pdf @@ -27,51 +33,33 @@ pub const GYRO_SENS: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4); /// * https://github.com/kriswiner/MPU6050/wiki/2014-Invensense-Developer%27s-Conference /// * rust MPU9250 driver on github 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)] -pub struct Specs; - -impl Specs { - // pub const ACCEL_SELF_TEST_MIN: u8 = -14; - pub const ACCEL_SELF_TEST_MAX: u8 = 14; -} - -#[allow(non_camel_case_types)] -#[derive(Copy, Clone, Debug)] -/// Register addresses -pub enum Registers { - /// 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, - // -} - -/// Slave address of Mpu6050 -pub const SLAVE_ADDR: u8 = 0x68; -/// Internal register to check slave addr -pub const WHOAMI: u8 = 0x75; /// Motion Threshold Register pub const MOT_THR: u8 = 0x1F; /// Motion Duration Detection Register pub const MOT_DUR: u8 = 0x20; +/// High Byte Register Gyro x orientation +pub const GYRO_REGX_H: u8 = 0x43; +/// High Byte Register Gyro y orientation +pub const GYRO_REGY_H: u8 = 0x45; +/// High Byte Register Gyro z orientation +pub const GYRO_REGZ_H: u8 = 0x47; +/// High Byte Register Calc roll +pub const ACC_REGX_H : u8= 0x3b; +/// High Byte Register Calc pitch +pub const ACC_REGY_H : u8= 0x3d; +/// High Byte Register Calc yaw +pub const ACC_REGZ_H : u8= 0x3f; +/// High Byte Register Temperature +pub const TEMP_OUT_H : u8= 0x41; +/// Slave address of Mpu6050 +pub const SLAVE_ADDR: u8 = 0x68; +/// Internal register to check slave addr +pub const WHOAMI: u8 = 0x75; /// Describes a bit block from bit number 'bit' to 'bit'+'length' pub struct BitBlock { @@ -79,12 +67,6 @@ pub struct BitBlock { pub length: u8 } -impl Registers { - pub fn addr(&self) -> u8 { - *self as u8 - } -} - #[allow(non_camel_case_types)] #[derive(Copy, Clone, Debug)] /// Register 26: Configuration (DLPF, External signal) @@ -294,9 +276,13 @@ impl PWR_MGMT_2 { #[derive(Copy, Clone, Debug, Eq, PartialEq)] /// Wake values pub enum LP_WAKE_CTRL { + /// 1.25 Hz _1P25 = 0, + /// 2.5 Hz _2P5, + /// 5 Hz _5, + /// 10 Hz _10, } @@ -304,11 +290,18 @@ pub enum LP_WAKE_CTRL { #[derive(Copy, Clone, Debug, Eq, PartialEq)] /// Accelerometer High Pass Filter Values pub enum ACCEL_HPF { + /// Cut off frequency: None _RESET = 0, + /// Cut off frequency: 5 Hz _5 = 1, + /// Cut off frequency: 2.5 Hz _2P5 = 2, + /// Cut off frequency: 1.25 Hz _1P25 = 3, + /// Cut off frequency: 0.63 Hz _0P63 = 4, + /// When triggered, the filter holds the present sample. The filter output will be the + /// difference between the input sample and the held sample _HOLD = 7 } @@ -331,13 +324,21 @@ impl From for ACCEL_HPF { #[derive(Copy, Clone, Debug, Eq, PartialEq)] /// Clock Source Select Values pub enum CLKSEL { + /// Internal 8MHz oscillator OSCILL = 0, + /// PLL with X axis gyroscope reference GXAXIS = 1, + /// PLL with Y axis gyroscope reference GYAXIS = 2, + /// PLL with Z axis gyroscope reference GZAXIS = 3, + /// PLL with external 32.768kHz reference EXT_32p7 = 4, + /// PLL with external 19.2MHz reference EXT_19P2 = 5, + /// Reserved RESERV = 6, + /// Stops the clock and keeps the timing generator in reset STOP = 7, } @@ -355,4 +356,80 @@ impl From for CLKSEL { _ => CLKSEL::GXAXIS } } -} \ No newline at end of file +} + +/// Defines accelerometer range/sensivity +#[derive(Debug, Eq, PartialEq, Copy, Clone)] +pub enum AccelRange { + /// 2G + G2 = 0, + /// 4G + G4, + /// 8G + G8, + /// 16G + G16, +} + +/// Defines gyro range/sensitivity +#[derive(Debug, Eq, PartialEq, Copy, Clone)] +pub enum GyroRange { + /// 250 degrees + D250 = 0, + /// 500 degrees + D500, + /// 1000 degrees + D1000, + /// 2000 degrees + 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 { + // Converts accelerometer range to correction/scaling factor, see register sheet + 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, + } + } +} + +impl GyroRange { + // Converts gyro range to correction/scaling factor, see register sheet + 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, + } + } +} diff --git a/src/lib.rs b/src/lib.rs index b1dcf21..d7d1793 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,53 +1,55 @@ -//! Mpu6050 sensor driver. +//! # Mpu6050 sensor driver. //! -//! Register sheet [here](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf), -//! Data sheet [here](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6500-Datasheet2.pdf) +//! `embedded_hal` based driver with i2c access to MPU6050 +//! +//! ### Misc +//! * [Register sheet](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf), +//! * [Data sheet](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6500-Datasheet2.pdf) //! //! To use this driver you must provide a concrete `embedded_hal` implementation. -//! This example uses `linux_embedded_hal` +//! This example uses `linux_embedded_hal`. +//! +//! **More Examples** can be found [here](https://github.com/juliangaal/mpu6050/tree/master/examples). //! ```no_run //! 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); -// } -// } +//! 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 sensor temp +//! let temp = mpu.get_temp()?; +//! printlnasd!("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); +//! } +//! } //! ``` #![no_std] +mod bits; pub mod device; -pub mod bits; -pub mod range; - -use crate::range::*; -use crate::device::{*, Registers::*}; +use crate::device::*; use libm::{powf, atan2f, sqrtf}; use nalgebra::{Vector3, Vector2}; use embedded_hal::{ @@ -198,6 +200,15 @@ where Ok(()) } + /// get current gyro range + pub fn get_gyro_range(&mut self) -> Result> { + let byte = self.read_bits(GYRO_CONFIG::ADDR, + GYRO_CONFIG::FS_SEL.bit, + GYRO_CONFIG::FS_SEL.length)?; + + Ok(GyroRange::from(byte)) + } + /// set accel range, and update sensitivy accordingly pub fn set_accel_range(&mut self, range: AccelRange) -> Result<(), Mpu6050Error> { self.write_bits(ACCEL_CONFIG::ADDR, @@ -218,15 +229,6 @@ where Ok(AccelRange::from(byte)) } - /// get current gyro range - pub fn get_gyro_range(&mut self) -> Result> { - let byte = self.read_bits(GYRO_CONFIG::ADDR, - GYRO_CONFIG::FS_SEL.bit, - GYRO_CONFIG::FS_SEL.length)?; - - Ok(GyroRange::from(byte)) - } - /// reset device pub fn reset_device>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error> { self.write_bit(PWR_MGMT_1::ADDR, PWR_MGMT_1::DEVICE_RESET, true)?; @@ -329,7 +331,7 @@ where /// Accelerometer readings in g pub fn get_acc(&mut self) -> Result, Mpu6050Error> { - let mut acc = self.read_rot(ACC_REGX_H.addr())?; + let mut acc = self.read_rot(ACC_REGX_H)?; acc /= self.acc_sensitivity; Ok(acc) @@ -337,17 +339,17 @@ where /// Gyro readings in rad/s pub fn get_gyro(&mut self) -> Result, Mpu6050Error> { - let mut gyro = self.read_rot(GYRO_REGX_H.addr())?; + let mut gyro = self.read_rot(GYRO_REGX_H)?; gyro *= PI_180 * self.gyro_sensitivity; Ok(gyro) } - /// Temp in degrees celcius + /// Sensor Temp in degrees celcius pub fn get_temp(&mut self) -> Result> { let mut buf: [u8; 2] = [0; 2]; - self.read_bytes(TEMP_OUT_H.addr(), &mut buf)?; + self.read_bytes(TEMP_OUT_H, &mut buf)?; let raw_temp = self.read_word_2c(&buf[0..2]) as f32; // According to revision 4.2 diff --git a/src/range.rs b/src/range.rs index 6bc0698..48a545a 100644 --- a/src/range.rs +++ b/src/range.rs @@ -1,68 +1 @@ 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