prep for crate version 1.4
This commit is contained in:
juliangaal
parent
d830eebbc6
commit
fc2a503f05
7 changed files with 199 additions and 257 deletions
91
README.md
91
README.md
|
|
@ -1,11 +1,17 @@
|
|||
# `mpu6050`  
|
||||
> 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<LinuxI2CError>> {
|
|||
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
|
||||
|
||||
```
|
||||
|
|
@ -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<LinuxI2CError>> {
|
||||
let i2c = I2cdev::new("/dev/i2c-1")
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@ fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
|
|||
let acc = mpu.get_acc_angles()?;
|
||||
println!("r/p: {:?}", acc);
|
||||
|
||||
// get temp
|
||||
// get sensor temp
|
||||
let temp = mpu.get_temp()?;
|
||||
println!("temp: {:?}c", temp);
|
||||
|
||||
|
|
|
|||
|
|
@ -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<LinuxI2CError>> {
|
|||
|
||||
// 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<LinuxI2CError>> {
|
|||
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);
|
||||
|
||||
|
|
|
|||
169
src/device.rs
169
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<u8> 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<u8> for CLKSEL {
|
|||
_ => CLKSEL::GXAXIS
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 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,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
106
src/lib.rs
106
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<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)?;
|
||||
//
|
||||
// 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<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)?;
|
||||
//!
|
||||
//! 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<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
|
||||
pub fn set_accel_range(&mut self, range: AccelRange) -> Result<(), Mpu6050Error<E>> {
|
||||
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<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
|
||||
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)?;
|
||||
|
|
@ -329,7 +331,7 @@ where
|
|||
|
||||
/// Accelerometer readings in g
|
||||
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;
|
||||
|
||||
Ok(acc)
|
||||
|
|
@ -337,17 +339,17 @@ where
|
|||
|
||||
/// Gyro readings in rad/s
|
||||
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;
|
||||
|
||||
Ok(gyro)
|
||||
}
|
||||
|
||||
/// Temp in degrees celcius
|
||||
/// Sensor Temp in degrees celcius
|
||||
pub fn get_temp(&mut self) -> Result<f32, Mpu6050Error<E>> {
|
||||
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
|
||||
|
|
|
|||
67
src/range.rs
67
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<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,
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in a new issue