ank/mpu6050
1
Fork 0
Code Issues Activity

Merge branch 'master' of github.com:juliangaal/mpu6050

Browse source
This commit is contained in:
juliangaal 2021-02-20 13:24:52 +01:00
commit f15f6158e4
8 changed files with 439 additions and 279 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

89
README.md
View file

@ -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) # `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 > 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
To use this driver you must provide a concrete `embedded_hal` implementation. Here's a
### Basic usage
[`linux_embedded_hal`](https://github.com/rust-embedded/linux-embedded-hal) example [`linux_embedded_hal`](https://github.com/rust-embedded/linux-embedded-hal) example
```rust ```rust
use mpu6050::*; use mpu6050::*;
@ -40,78 +46,3 @@ fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
} }
} }
``` ```
### 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

33
examples/motion_detection.rs Normal file
View file

@ -0,0 +1,33 @@
use mpu6050::{*, device::MOT_DETECT_STATUS};
use linux_embedded_hal::{I2cdev, Delay};
use i2cdev::linux::LinuxI2CError;
use embedded_hal::blocking::delay::DelayMs;
fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
let i2c = I2cdev::new("/dev/i2c-1")
.map_err(Mpu6050Error::I2c)?;
let mut delay = Delay;
let mut mpu = Mpu6050::new(i2c);
mpu.init(&mut delay)?;
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(())
}

2
examples/simple.rs
View file

@ -16,7 +16,7 @@ fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
let acc = mpu.get_acc_angles()?; let acc = mpu.get_acc_angles()?;
println!("r/p: {:?}", acc); println!("r/p: {:?}", acc);
// get temp // get sensor temp
let temp = mpu.get_temp()?; let temp = mpu.get_temp()?;
println!("temp: {:?}c", temp); println!("temp: {:?}c", temp);

18
examples/test.rs
View file

@ -1,4 +1,4 @@
use mpu6050::*; use mpu6050::{*, device::*};
use linux_embedded_hal::{I2cdev, Delay}; use linux_embedded_hal::{I2cdev, Delay};
use i2cdev::linux::LinuxI2CError; use i2cdev::linux::LinuxI2CError;
@ -18,15 +18,15 @@ fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
// Test gyro config // Test gyro config
println!("Test gyro config"); println!("Test gyro config");
assert_eq!(mpu.get_gyro_range()?, range::GyroRange::D250); assert_eq!(mpu.get_gyro_range()?, GyroRange::D250);
mpu.set_gyro_range(range::GyroRange::D500)?; mpu.set_gyro_range(GyroRange::D500)?;
assert_eq!(mpu.get_gyro_range()?, range::GyroRange::D500); assert_eq!(mpu.get_gyro_range()?, GyroRange::D500);
// Test accel config // Test accel config
println!("Test accel config"); println!("Test accel config");
assert_eq!(mpu.get_accel_range()?, range::AccelRange::G2); assert_eq!(mpu.get_accel_range()?, AccelRange::G2);
mpu.set_accel_range(range::AccelRange::G4)?; mpu.set_accel_range(AccelRange::G4)?;
assert_eq!(mpu.get_accel_range()?, range::AccelRange::G4); assert_eq!(mpu.get_accel_range()?, AccelRange::G4);
// accel_hpf: per default RESET/no filter, see ACCEL_CONFIG // accel_hpf: per default RESET/no filter, see ACCEL_CONFIG
println!("Test accel hpf"); println!("Test accel hpf");
@ -81,8 +81,8 @@ fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
println!("Test reset"); println!("Test reset");
mpu.reset_device(&mut delay)?; mpu.reset_device(&mut delay)?;
assert_eq!(mpu.get_accel_hpf()?, ACCEL_HPF::_RESET); assert_eq!(mpu.get_accel_hpf()?, ACCEL_HPF::_RESET);
assert_eq!(mpu.get_accel_range()?, range::AccelRange::G2); assert_eq!(mpu.get_accel_range()?, AccelRange::G2);
assert_eq!(mpu.get_gyro_range()?, range::GyroRange::D250); assert_eq!(mpu.get_gyro_range()?, GyroRange::D250);
assert_eq!(mpu.get_sleep_enabled()?, true); assert_eq!(mpu.get_sleep_enabled()?, true);
assert_eq!(mpu.get_temp_enabled()?, true); assert_eq!(mpu.get_temp_enabled()?, true);

40
misc/status.md
View file

@ -19,7 +19,7 @@
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x17] ZG_OFFS_USRH| R/W | [15:0] ZG_OFFS_USR| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x17] ZG_OFFS_USRH| R/W | [15:0] ZG_OFFS_USR|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x18] ZG_OFFS_USRL| R/W || | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x18] ZG_OFFS_USRL| R/W ||
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x19] SMPLRT_DIV| R/W | [7:0] SMPLRT_DIV| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x19] SMPLRT_DIV| R/W | [7:0] SMPLRT_DIV|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x1A] CONFIG| R/W | [5:3] EXT_SYNC_SET [2:0] DLPF_CFG| | <ul><li> -[x] </li></ul>|<ul><li> -[ ] </li></ul>|[0x1A] CONFIG| R/W | [5:3] EXT_SYNC_SET [2:0] DLPF_CFG|
| <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x1B] GYRO_CONFIG| R/W | [7] XG_ST [6] YG_ST [5] ZG_ST [4:3] FS_SEL| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x1B] GYRO_CONFIG| R/W | [7] XG_ST [6] YG_ST [5] ZG_ST [4:3] FS_SEL|
| <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x1C] ACCEL_CONFIG| R/W | [7] XA_ST [6] YA_ST [5] ZA_ST [4:3] AFS_SEL [2:0] ACCEL_HPF| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x1C] ACCEL_CONFIG| R/W | [7] XA_ST [6] YA_ST [5] ZA_ST [4:3] AFS_SEL [2:0] ACCEL_HPF|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x1D] FF_THR| R/W | [7:0] FF_THR| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x1D] FF_THR| R/W | [7:0] FF_THR|
@ -48,24 +48,24 @@
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x35] I2C_SLV4_DI| R/W | [7:0] I2C_SLV4_DI| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x35] I2C_SLV4_DI| R/W | [7:0] I2C_SLV4_DI|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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| | <ul><li> -[x] </li></ul>|<ul><li> -[ ] </li></ul>|[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|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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| | <ul><li> -[x] </li></ul>|<ul><li> -[ ] </li></ul>|[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|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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| | <ul><li> -[x] </li></ul>|<ul><li> -[ ] </li></ul>|[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|
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x3B] ACCEL_XOUT_H| RO| [15:0] ACCEL_XOUT| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x3B] ACCEL_XOUT_H| RO| [15:0] ACCEL_XOUT|
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x3C] ACCEL_XOUT_L| RO| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x3C] ACCEL_XOUT_L| RO|
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x3D] ACCEL_YOUT_H| RO| [15:0] ACCEL_YOUT| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x3D] ACCEL_YOUT_H| RO| [15:0] ACCEL_YOUT|
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x3E] ACCEL_YOUT_L| RO| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x3E] ACCEL_YOUT_L| RO|
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x3F] ACCEL_ZOUT_H| RO| [15:0] ACCEL_ZOUT| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x3F] ACCEL_ZOUT_H| RO| [15:0] ACCEL_ZOUT|
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x40] ACCEL_ZOUT_L| RO|| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x40] ACCEL_ZOUT_L| RO||
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x41] TEMP_OUT_H| RO| [15:0] TEMP_OUT| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x41] TEMP_OUT_H| RO| [15:0] TEMP_OUT|
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x42] TEMP_OUT_L| RO|| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x42] TEMP_OUT_L| RO||
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x43] GYRO_XOUT_H| RO | [15:0] GYRO_XOUT| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x43] GYRO_XOUT_H| RO | [15:0] GYRO_XOUT|
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x44] GYRO_XOUT_L| RO|| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x44] GYRO_XOUT_L| RO||
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x45] GYRO_YOUT_H| RO| [15:0] GYRO_YOUT| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x45] GYRO_YOUT_H| RO| [15:0] GYRO_YOUT|
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x46] GYRO_YOUT_L| RO|| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x46] GYRO_YOUT_L| RO||
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x47] GYRO_ZOUT_H| RO| [15:0] GYRO_ZOUT| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x47] GYRO_ZOUT_H| RO| [15:0] GYRO_ZOUT|
| <ul><li> -[x] </li><li> -[x] </li></ul>|[0x48] GYRO_ZOUT_L| RO|| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x48] GYRO_ZOUT_L| RO||
| <ul><li> -[ ] </ul></li>|<ul><li> -[ ] </li></ul>|[0x49] EXT_SENS_DATA_00| RO |[7:0] EXT_SENS_DATA_00| | <ul><li> -[ ] </ul></li>|<ul><li> -[ ] </li></ul>|[0x49] EXT_SENS_DATA_00| RO |[7:0] EXT_SENS_DATA_00|
| <ul><li> -[ ] </ul></li>|<ul><li> -[ ] </li></ul>|[0x4A] EXT_SENS_DATA_01| RO |[7:0] EXT_SENS_DATA_01| | <ul><li> -[ ] </ul></li>|<ul><li> -[ ] </li></ul>|[0x4A] EXT_SENS_DATA_01| RO |[7:0] EXT_SENS_DATA_01|
| <ul><li> -[ ] </ul></li>|<ul><li> -[ ] </li></ul>|[0x4B] EXT_SENS_DATA_02| RO |[7:0] EXT_SENS_DATA_02| | <ul><li> -[ ] </ul></li>|<ul><li> -[ ] </li></ul>|[0x4B] EXT_SENS_DATA_02| RO |[7:0] EXT_SENS_DATA_02|
@ -90,14 +90,14 @@
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x5E] EXT_SENS_DATA_21| RO |[7:0] EXT_SENS_DATA_21| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x5E] EXT_SENS_DATA_21| RO |[7:0] EXT_SENS_DATA_21|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x5F] EXT_SENS_DATA_22| RO |[7:0] EXT_SENS_DATA_22| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x5F] EXT_SENS_DATA_22| RO |[7:0] EXT_SENS_DATA_22|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x60] EXT_SENS_DATA_23| RO |[7:0] EXT_SENS_DATA_23| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x60] EXT_SENS_DATA_23| RO |[7:0] EXT_SENS_DATA_23|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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| | <ul><li> -[x] </li></ul>|<ul><li> -[ ] </li></ul>|[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|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x63] I2C_SLV0_DO| R/W | [7:0] I2C_SLV0_DO| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x63] I2C_SLV0_DO| R/W | [7:0] I2C_SLV0_DO|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x64] I2C_SLV1_DO| R/W | [7:0] I2C_SLV1_DO| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x64] I2C_SLV1_DO| R/W | [7:0] I2C_SLV1_DO|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x65] I2C_SLV2_DO| R/W | [7:0] I2C_SLV2_DO| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x65] I2C_SLV2_DO| R/W | [7:0] I2C_SLV2_DO|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x66] I2C_SLV3_DO| R/W | [7:0] I2C_SLV3_DO| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x66] I2C_SLV3_DO| R/W | [7:0] I2C_SLV3_DO|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x68] SIGNAL_PATH_RESET| R/W | [2] GYRO_RESET [1] ACCEL_RESET [0] TEMP_RESET| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x68] SIGNAL_PATH_RESET| R/W | [2] GYRO_RESET [1] ACCEL_RESET [0] TEMP_RESET|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[0x69] MOT_DETECT_CTRL| R/W | [5:4] ACCEL_ON_DELAY [3:2] FF_COUNT [1:0] MOT_COUNT| | <ul><li> -[x] </li></ul>|<ul><li> -[ ] </li></ul>|[0x69] MOT_DETECT_CTRL| R/W | [5:4] ACCEL_ON_DELAY [3:2] FF_COUNT [1:0] MOT_COUNT|
| <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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| | <ul><li> -[ ] </li></ul>|<ul><li> -[ ] </li></ul>|[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|
| <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x6B] PWR_MGMT_1| R/W | [7] DEVICE_RESET [6] SLEEP [5] CYCLE [3] TEMP_DIS [2:0] CLK_SEL| | <ul><li> -[x] </li></ul>|<ul><li> -[x] </li></ul>|[0x6B] PWR_MGMT_1| R/W | [7] DEVICE_RESET [6] SLEEP [5] CYCLE [3] TEMP_DIS [2:0] CLK_SEL|
| <ul><li> -[x] </li></ul>|<ul><li> -[ ] </li></ul>|[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| | <ul><li> -[x] </li></ul>|<ul><li> -[ ] </li></ul>|[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|

328
src/device.rs
View file

@ -1,12 +1,18 @@
//! All constants used in the driver, mostly register addresses //! All device 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
//! //!
//! 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 /// Gyro Sensitivity
/// ///
/// Measurements are scaled like this: /// Measurements are scaled like this:
/// x * range/2**(resolution-1) or x / (2**(resolution-1) / range) /// x * range/2**(resolution-1) or x / (2**(resolution-1) / range)
///
/// Sources: /// Sources:
/// * https://www.nxp.com/docs/en/application-note/AN3461.pdf /// * https://www.nxp.com/docs/en/application-note/AN3461.pdf
/// * https://theccontinuum.com/2012/09/24/arduino-imu-pitch-roll-from-accelerometer/ /// * https://theccontinuum.com/2012/09/24/arduino-imu-pitch-roll-from-accelerometer/
@ -18,6 +24,7 @@ pub const GYRO_SENS: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4);
/// Accelerometer Sensitivity /// Accelerometer Sensitivity
/// ///
/// Measurements are scaled like this: /// Measurements are scaled like this:
///
/// x * range/2**(resolution-1) or x / (2**(resolution-1) / range) /// x * range/2**(resolution-1) or x / (2**(resolution-1) / range)
/// Sources: /// Sources:
/// * https://www.nxp.com/docs/en/application-note/AN3461.pdf /// * https://www.nxp.com/docs/en/application-note/AN3461.pdf
@ -26,47 +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 /// * https://github.com/kriswiner/MPU6050/wiki/2014-Invensense-Developer%27s-Conference
/// * rust MPU9250 driver on github /// * rust MPU9250 driver on github
pub const ACCEL_SENS: (f32, f32, f32, f32) = (16384., 8192., 4096., 2048.); pub const ACCEL_SENS: (f32, f32, f32, f32) = (16384., 8192., 4096., 2048.);
/// Temperature Offset /// Temperature Offset
pub const TEMP_OFFSET: f32 = 36.53; pub const TEMP_OFFSET: f32 = 36.53;
/// Temperature Sensitivity /// Temperature Sensitivity
pub const TEMP_SENSITIVITY: f32 = 340.; pub const TEMP_SENSITIVITY: f32 = 340.;
#[allow(non_camel_case_types)] /// Motion Threshold Register
#[derive(Copy, Clone, Debug)] pub const MOT_THR: u8 = 0x1F;
pub struct Specs; /// Motion Duration Detection Register
pub const MOT_DUR: u8 = 0x20;
impl Specs { /// High Byte Register Gyro x orientation
// pub const ACCEL_SELF_TEST_MIN: u8 = -14; pub const GYRO_REGX_H: u8 = 0x43;
pub const ACCEL_SELF_TEST_MAX: u8 = 14; /// High Byte Register Gyro y orientation
} pub const GYRO_REGY_H: u8 = 0x45;
/// High Byte Register Gyro z orientation
#[allow(non_camel_case_types)] pub const GYRO_REGZ_H: u8 = 0x47;
#[derive(Copy, Clone, Debug)] /// High Byte Register Calc roll
/// Register addresses pub const ACC_REGX_H : u8= 0x3b;
pub enum Registers { /// High Byte Register Calc pitch
/// Slave address of Mpu6050 pub const ACC_REGY_H : u8= 0x3d;
SLAVE_ADDR = 0x68, /// High Byte Register Calc yaw
/// Internal register to check slave addr pub const ACC_REGZ_H : u8= 0x3f;
WHOAMI = 0x75, /// High Byte Register Temperature
/// High Byte Register Gyro x orientation pub const TEMP_OUT_H : u8= 0x41;
GYRO_REGX_H = 0x43, /// Slave address of Mpu6050
/// High Byte Register Gyro y orientation pub const SLAVE_ADDR: u8 = 0x68;
GYRO_REGY_H = 0x45, /// Internal register to check slave addr
/// High Byte Register Gyro z orientation pub const WHOAMI: u8 = 0x75;
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,
/// Internal clock
POWER_MGMT_2 = 0x6c,
}
/// Describes a bit block from bit number 'bit' to 'bit'+'length' /// Describes a bit block from bit number 'bit' to 'bit'+'length'
pub struct BitBlock { pub struct BitBlock {
@ -74,10 +67,18 @@ pub struct BitBlock {
pub length: u8 pub length: u8
} }
impl Registers { #[allow(non_camel_case_types)]
pub fn addr(&self) -> u8 { #[derive(Copy, Clone, Debug)]
*self as u8 /// 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)] #[allow(non_camel_case_types)]
@ -119,7 +120,117 @@ impl ACCEL_CONFIG {
#[allow(non_camel_case_types)] #[allow(non_camel_case_types)]
#[derive(Copy, Clone, Debug)] #[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; pub struct PWR_MGMT_1;
impl PWR_MGMT_1 { impl PWR_MGMT_1 {
@ -137,15 +248,60 @@ impl PWR_MGMT_1 {
pub const CLKSEL: BitBlock = BitBlock { bit: 2, length: 3 }; 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 {
/// 1.25 Hz
_1P25 = 0,
/// 2.5 Hz
_2P5,
/// 5 Hz
_5,
/// 10 Hz
_10,
}
#[allow(non_camel_case_types)] #[allow(non_camel_case_types)]
#[derive(Copy, Clone, Debug, Eq, PartialEq)] #[derive(Copy, Clone, Debug, Eq, PartialEq)]
/// Accelerometer High Pass Filter Values /// Accelerometer High Pass Filter Values
pub enum ACCEL_HPF { pub enum ACCEL_HPF {
/// Cut off frequency: None
_RESET = 0, _RESET = 0,
/// Cut off frequency: 5 Hz
_5 = 1, _5 = 1,
/// Cut off frequency: 2.5 Hz
_2P5 = 2, _2P5 = 2,
/// Cut off frequency: 1.25 Hz
_1P25 = 3, _1P25 = 3,
/// Cut off frequency: 0.63 Hz
_0P63 = 4, _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 _HOLD = 7
} }
@ -168,13 +324,21 @@ impl From<u8> for ACCEL_HPF {
#[derive(Copy, Clone, Debug, Eq, PartialEq)] #[derive(Copy, Clone, Debug, Eq, PartialEq)]
/// Clock Source Select Values /// Clock Source Select Values
pub enum CLKSEL { pub enum CLKSEL {
/// Internal 8MHz oscillator
OSCILL = 0, OSCILL = 0,
/// PLL with X axis gyroscope reference
GXAXIS = 1, GXAXIS = 1,
/// PLL with Y axis gyroscope reference
GYAXIS = 2, GYAXIS = 2,
/// PLL with Z axis gyroscope reference
GZAXIS = 3, GZAXIS = 3,
/// PLL with external 32.768kHz reference
EXT_32p7 = 4, EXT_32p7 = 4,
/// PLL with external 19.2MHz reference
EXT_19P2 = 5, EXT_19P2 = 5,
/// Reserved
RESERV = 6, RESERV = 6,
/// Stops the clock and keeps the timing generator in reset
STOP = 7, STOP = 7,
} }
@ -193,3 +357,79 @@ impl From<u8> for CLKSEL {
} }
} }
} }
/// 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<u8> for GyroRange {
fn from(range: u8) -> Self
{
match range {
0 => GyroRange::D250,
1 => GyroRange::D500,
2 => GyroRange::D1000,
3 => GyroRange::D2000,
_ => GyroRange::D250
}
}
}
impl From<u8> for AccelRange {
fn from(range: u8) -> Self
{
match range {
0 => AccelRange::G2,
1 => AccelRange::G4,
2 => AccelRange::G8,
3 => AccelRange::G16,
_ => AccelRange::G2
}
}
}
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,
}
}
}

137
src/lib.rs
View file

@ -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), //! `embedded_hal` based driver with i2c access to MPU6050
//! Data sheet [here](https://www.invensense.com/wp-content/uploads/2015/02/MPU-6500-Datasheet2.pdf) //!
//! ### 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. //! 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 //! ```no_run
//! use mpu6050::*; //! use mpu6050::*;
// use linux_embedded_hal::{I2cdev, Delay}; //! use linux_embedded_hal::{I2cdev, Delay};
// use i2cdev::linux::LinuxI2CError; //! use i2cdev::linux::LinuxI2CError;
// //!
// fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> { //! fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
// let i2c = I2cdev::new("/dev/i2c-1") //! let i2c = I2cdev::new("/dev/i2c-1")
// .map_err(Mpu6050Error::I2c)?; //! .map_err(Mpu6050Error::I2c)?;
// //!
// let mut delay = Delay; //! let mut delay = Delay;
// let mut mpu = Mpu6050::new(i2c); //! let mut mpu = Mpu6050::new(i2c);
// //!
// mpu.init(&mut delay)?; //! mpu.init(&mut delay)?;
// //!
// loop { //! loop {
// // get roll and pitch estimate //! // get roll and pitch estimate
// let acc = mpu.get_acc_angles()?; //! let acc = mpu.get_acc_angles()?;
// println!("r/p: {:?}", acc); //! println!("r/p: {:?}", acc);
// //!
// // get temp //! // get sensor temp
// let temp = mpu.get_temp()?; //! let temp = mpu.get_temp()?;
// println!("temp: {:?}c", temp); //! printlnasd!("temp: {:?}c", temp);
// //!
// // get gyro data, scaled with sensitivity //! // get gyro data, scaled with sensitivity
// let gyro = mpu.get_gyro()?; //! let gyro = mpu.get_gyro()?;
// println!("gyro: {:?}", gyro); //! println!("gyro: {:?}", gyro);
// //!
// // get accelerometer data, scaled with sensitivity //! // get accelerometer data, scaled with sensitivity
// let acc = mpu.get_acc()?; //! let acc = mpu.get_acc()?;
// println!("acc: {:?}", acc); //! println!("acc: {:?}", acc);
// } //! }
// } //! }
//! ``` //! ```
#![no_std] #![no_std]
mod bits;
pub mod device; 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 libm::{powf, atan2f, sqrtf};
use nalgebra::{Vector3, Vector2}; use nalgebra::{Vector3, Vector2};
use embedded_hal::{ use embedded_hal::{
@ -140,13 +142,34 @@ where
/// Verifies device to address 0x68 with WHOAMI.addr() Register /// Verifies device to address 0x68 with WHOAMI.addr() Register
fn verify(&mut self) -> Result<(), Mpu6050Error<E>> { fn verify(&mut self) -> Result<(), Mpu6050Error<E>> {
let address = self.read_byte(WHOAMI.addr())?; let address = self.read_byte(WHOAMI)?;
if address != SLAVE_ADDR.addr() { if address != SLAVE_ADDR {
return Err(Mpu6050Error::InvalidChipId(address)); return Err(Mpu6050Error::InvalidChipId(address));
} }
Ok(()) 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<E>> {
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<bool, Mpu6050Error<E>> {
Ok(self.read_bit(INT_STATUS::ADDR, INT_STATUS::MOT_INT)? != 0)
}
/// set accel high pass filter mode /// set accel high pass filter mode
pub fn set_accel_hpf(&mut self, mode: ACCEL_HPF) -> Result<(), Mpu6050Error<E>> { pub fn set_accel_hpf(&mut self, mode: ACCEL_HPF) -> Result<(), Mpu6050Error<E>> {
Ok( Ok(
@ -177,6 +200,15 @@ where
Ok(()) Ok(())
} }
/// get current gyro range
pub fn get_gyro_range(&mut self) -> Result<GyroRange, Mpu6050Error<E>> {
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 /// set accel range, and update sensitivy accordingly
pub fn set_accel_range(&mut self, range: AccelRange) -> Result<(), Mpu6050Error<E>> { pub fn set_accel_range(&mut self, range: AccelRange) -> Result<(), Mpu6050Error<E>> {
self.write_bits(ACCEL_CONFIG::ADDR, self.write_bits(ACCEL_CONFIG::ADDR,
@ -197,15 +229,6 @@ where
Ok(AccelRange::from(byte)) Ok(AccelRange::from(byte))
} }
/// get current gyro range
pub fn get_gyro_range(&mut self) -> Result<GyroRange, Mpu6050Error<E>> {
let byte = self.read_bits(GYRO_CONFIG::ADDR,
GYRO_CONFIG::FS_SEL.bit,
GYRO_CONFIG::FS_SEL.length)?;
Ok(GyroRange::from(byte))
}
/// reset device /// reset device
pub fn reset_device<D: DelayMs<u8>>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error<E>> { pub fn reset_device<D: DelayMs<u8>>(&mut self, delay: &mut D) -> Result<(), Mpu6050Error<E>> {
self.write_bit(PWR_MGMT_1::ADDR, PWR_MGMT_1::DEVICE_RESET, true)?; self.write_bit(PWR_MGMT_1::ADDR, PWR_MGMT_1::DEVICE_RESET, true)?;
@ -308,7 +331,7 @@ where
/// Accelerometer readings in g /// Accelerometer readings in g
pub fn get_acc(&mut self) -> Result<Vector3<f32>, Mpu6050Error<E>> { pub fn get_acc(&mut self) -> Result<Vector3<f32>, Mpu6050Error<E>> {
let mut acc = self.read_rot(ACC_REGX_H.addr())?; let mut acc = self.read_rot(ACC_REGX_H)?;
acc /= self.acc_sensitivity; acc /= self.acc_sensitivity;
Ok(acc) Ok(acc)
@ -316,17 +339,17 @@ where
/// Gyro readings in rad/s /// Gyro readings in rad/s
pub fn get_gyro(&mut self) -> Result<Vector3<f32>, Mpu6050Error<E>> { pub fn get_gyro(&mut self) -> Result<Vector3<f32>, Mpu6050Error<E>> {
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; gyro *= PI_180 * self.gyro_sensitivity;
Ok(gyro) Ok(gyro)
} }
/// Temp in degrees celcius /// Sensor Temp in degrees celcius
pub fn get_temp(&mut self) -> Result<f32, Mpu6050Error<E>> { pub fn get_temp(&mut self) -> Result<f32, Mpu6050Error<E>> {
let mut buf: [u8; 2] = [0; 2]; 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; let raw_temp = self.read_word_2c(&buf[0..2]) as f32;
// According to revision 4.2 // According to revision 4.2
@ -335,7 +358,7 @@ where
/// Writes byte to register /// Writes byte to register
pub fn write_byte(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6050Error<E>> { pub fn write_byte(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6050Error<E>> {
self.i2c.write(SLAVE_ADDR.addr(), &[reg, byte]) self.i2c.write(SLAVE_ADDR, &[reg, byte])
.map_err(Mpu6050Error::I2c)?; .map_err(Mpu6050Error::I2c)?;
// delay disabled for dev build // delay disabled for dev build
// TODO: check effects with physical unit // TODO: check effects with physical unit
@ -376,14 +399,14 @@ where
/// Reads byte from register /// Reads byte from register
pub fn read_byte(&mut self, reg: u8) -> Result<u8, Mpu6050Error<E>> { pub fn read_byte(&mut self, reg: u8) -> Result<u8, Mpu6050Error<E>> {
let mut byte: [u8; 1] = [0; 1]; 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)?; .map_err(Mpu6050Error::I2c)?;
Ok(byte[0]) Ok(byte[0])
} }
/// Reads series of bytes into buf from specified reg /// Reads series of bytes into buf from specified reg
pub fn read_bytes(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), Mpu6050Error<E>> { pub fn read_bytes(&mut self, reg: u8, buf: &mut [u8]) -> Result<(), Mpu6050Error<E>> {
self.i2c.write_read(SLAVE_ADDR.addr(), &[reg], buf) self.i2c.write_read(SLAVE_ADDR, &[reg], buf)
.map_err(Mpu6050Error::I2c)?; .map_err(Mpu6050Error::I2c)?;
Ok(()) Ok(())
} }

67
src/range.rs
View file

@ -1,68 +1 @@
use crate::device::*; 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<u8> for GyroRange {
fn from(range: u8) -> Self
{
match range {
0 => GyroRange::D250,
1 => GyroRange::D500,
2 => GyroRange::D1000,
3 => GyroRange::D2000,
_ => GyroRange::D250
}
}
}
impl From<u8> for AccelRange {
fn from(range: u8) -> Self
{
match range {
0 => AccelRange::G2,
1 => AccelRange::G4,
2 => AccelRange::G8,
3 => AccelRange::G16,
_ => AccelRange::G2
}
}
}
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,
}
}
}