diff --git a/examples/motion_detection.rs b/examples/motion_detection.rs new file mode 100644 index 0000000..d1569fb --- /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()?; + + 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..b1dcf21 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) -> 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( @@ -335,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 @@ -376,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(()) }