ank/mpu6050
1
Fork 0
Code Issues Activity

Merge pull request #24 from juliangaal/dev

Browse source 
bump dependencies, rm buggy bias calc
This commit is contained in:
Julian Gaal 2021-02-15 19:02:10 +01:00 committed by GitHub
commit ccb1a24d42
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 182 additions and 490 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

87
.circleci/config.yml
View file

@ -7,56 +7,101 @@ jobs:
- checkout - checkout
- run: - run:
name: build and test name: build and test
command: cargo build && cargo test command: cargo build
threefour: fournine:
docker: docker:
- image: circleci/rust:1.34.0 - image: circleci/rust:1.49.0
steps: steps:
- checkout - checkout
- run: - run:
name: build and test name: build and test
command: cargo build && cargo test command: cargo build
threethree: foureight:
docker: docker:
- image: circleci/rust:1.33.0 - image: circleci/rust:1.48.0
steps: steps:
- checkout - checkout
- run: - run:
name: build and test name: build and test
command: cargo build && cargo test command: cargo build
threetwo: fourseven:
docker: docker:
- image: circleci/rust:1.33.0 - image: circleci/rust:1.47.0
steps: steps:
- checkout - checkout
- run: - run:
name: build and test name: build and test
command: cargo build && cargo test command: cargo build
threeone: foursix:
docker: docker:
- image: circleci/rust:1.33.0 - image: circleci/rust:1.46.0
steps: steps:
- checkout - checkout
- run: - run:
name: build and test name: build and test
command: cargo build && cargo test command: cargo build
threezero: fourfive:
docker: docker:
- image: circleci/rust:1.33.0 - image: circleci/rust:1.45.0
steps: steps:
- checkout - checkout
- run: - run:
name: build and test name: build and test
command: cargo build && cargo test command: cargo build
fourfour:
docker:
- image: circleci/rust:1.44.0
steps:
- checkout
- run:
name: build and test
command: cargo build
fourthree:
docker:
- image: circleci/rust:1.43.0
steps:
- checkout
- run:
name: build and test
command: cargo build
fourtwo:
docker:
- image: circleci/rust:1.42.0
steps:
- checkout
- run:
name: build and test
command: cargo build
fourone:
docker:
- image: circleci/rust:1.40.0
steps:
- checkout
- run:
name: build and test
command: cargo build
fourzero:
docker:
- image: circleci/rust:1.40.0
steps:
- checkout
- run:
name: build and test
command: cargo build
workflows: workflows:
version: 2 version: 2
build_and_test: build_and_test:
jobs: jobs:
- latest - latest
- threefour - fournine
- threethree - foureight
- threetwo - fourseven
- threeone - foursix
- threezero - fourfive
- fourfour
- fourthree
- fourtwo
- fourone
- fourzero

8
Cargo.toml
View file

@ -11,13 +11,13 @@ keywords = ["mpu6050", "imu", "embedded"]
license = "MIT" license = "MIT"
[dependencies] [dependencies]
embedded-hal = "0.2.2" embedded-hal = "0.2.4"
libm = "0.2.1" libm = "0.2.1"
[dependencies.nalgebra] [dependencies.nalgebra]
default-features = false default-features = false
version = "0.18.0" version = "0.24.1"
[dev-dependencies] [dev-dependencies]
i2cdev = "0.4.2" i2cdev = "0.4.4"
linux-embedded-hal = "0.2.2" linux-embedded-hal = "0.3.0"

79
README.md
View file

@ -1,11 +1,6 @@
# `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
### Status
* Retrieving raw/averaged data works just fine
* **Don't use/rely on (software) calibration at this point in time**
[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) [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)
Visualization options for testing and development in [viz branch](https://github.com/juliangaal/mpu6050/tree/viz/viz) Visualization options for testing and development in [viz branch](https://github.com/juliangaal/mpu6050/tree/viz/viz)
@ -25,50 +20,30 @@ fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
let mut mpu = Mpu6050::new(i2c); let mut mpu = Mpu6050::new(i2c);
mpu.init(&mut delay)?; mpu.init(&mut delay)?;
mpu.soft_calib(Steps(100))?;
mpu.calc_variance(Steps(50))?;
println!("Calibrated with bias: {:?}", mpu.get_bias().unwrap());
println!("Calculated variance: {:?}", mpu.get_variance().unwrap());
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 roll and pitch estimate - averaged accross n readings (steps)
let acc = mpu.get_acc_angles_avg(Steps(5))?;
println!("r/p avg: {:?}", acc);
// get temp // get temp
let temp = mpu.get_temp()?; let temp = mpu.get_temp()?;
println!("temp: {:?}c", temp); println!("temp: {:?}c", temp);
// get temp - averages across n readings (steps)
let temp = mpu.get_temp_avg(Steps(5))?;
println!("temp avg: {:?}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 gyro data, scaled with sensitivity - averaged across n readings (steps)
let gyro = mpu.get_gyro_avg(Steps(5))?;
println!("gyro avg: {:?}", 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);
// get accelerometer data, scaled with sensitivity - averages across n readings (steps)
let acc = mpu.get_acc_avg(Steps(5))?;
println!("acc avg: {:?}", acc);
} }
} }
``` ```
#### Compile linux example (Raspberry Pi 3B) ### Linux example (Raspberry Pi 3B)
#### Cross compile
Requirements: Requirements:
```bash ```bash
$ apt-get install -qq gcc-arm-linux-gnueabihf libc6-armhf-cross libc6-dev-armhf-cross $ apt-get install -qq gcc-arm-linux-gnueabihf libc6-armhf-cross libc6-dev-armhf-cross
@ -84,6 +59,47 @@ cross-compile with
```bash ```bash
$ cargo build --examples --target=armv7-unknown-linux-gnueabihf $ 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 ## TODO
- [x] init with default settings - [x] init with default settings
@ -92,14 +108,9 @@ $ cargo build --examples --target=armv7-unknown-linux-gnueabihf
- [ ] custom device initialization - [ ] custom device initialization
- [x] device verification - [x] device verification
- [x] basic feature support as described [here](https://github.com/Tijndagamer/mpu6050/blob/master/mpu6050/mpu6050.py) - [x] basic feature support as described [here](https://github.com/Tijndagamer/mpu6050/blob/master/mpu6050/mpu6050.py)
- [x] read gyro data - [x] rename constants to better match datasheet
- [x] read acc data
- [x] software calibration
- [x] software measurement variance estimation
- [x] roll, pitch estimation accelerometer only
- [x] read temp data
- [ ] rename constants to better match datasheet
- [ ] complementary filter for roll, pitch estimate, possible on device? - [ ] 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 - [ ] 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) - [ ] 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 - [x] plotting [csv data](https://plot.ly/python/plot-data-from-csv/)for testing? -> See viz branch

21
examples/linux.rs
View file

@ -10,43 +10,22 @@ fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
let mut mpu = Mpu6050::new(i2c); let mut mpu = Mpu6050::new(i2c);
mpu.init(&mut delay)?; mpu.init(&mut delay)?;
mpu.soft_calib(Steps(100))?;
mpu.calc_variance(Steps(50))?;
println!("Calibrated with bias: {:?}", mpu.get_bias().unwrap());
println!("Calculated variance: {:?}", mpu.get_variance().unwrap());
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 roll and pitch estimate - averaged accross n readings (steps)
let acc = mpu.get_acc_angles_avg(Steps(5))?;
println!("r/p avg: {:?}", acc);
// get temp // get temp
let temp = mpu.get_temp()?; let temp = mpu.get_temp()?;
println!("temp: {:?}c", temp); println!("temp: {:?}c", temp);
// get temp - averages across n readings (steps)
let temp = mpu.get_temp_avg(Steps(5))?;
println!("temp avg: {:?}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 gyro data, scaled with sensitivity - averaged across n readings (steps)
let gyro = mpu.get_gyro_avg(Steps(5))?;
println!("gyro avg: {:?}", 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);
// get accelerometer data, scaled with sensitivity - averages across n readings (steps)
let acc = mpu.get_acc_avg(Steps(5))?;
println!("acc avg: {:?}", acc);
} }
} }

429
src/lib.rs
View file

@ -7,57 +7,36 @@
//! This example uses `linux_embedded_hal` //! This example uses `linux_embedded_hal`
//! ```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)?;
//! mpu.soft_calib(Steps(100))?; //
//! mpu.calc_variance(Steps(50))?; // loop {
//! // // get roll and pitch estimate
//! println!("Calibrated with bias: {:?}", mpu.get_bias().unwrap()); // let acc = mpu.get_acc_angles()?;
//! println!("Calculated variance: {:?}", mpu.get_variance().unwrap()); // println!("r/p: {:?}", acc);
//! //
//! loop { // // get temp
//! // get roll and pitch estimate // let temp = mpu.get_temp()?;
//! let acc = mpu.get_acc_angles()?; // println!("temp: {:?}c", temp);
//! println!("r/p: {:?}", acc); //
//! // // get gyro data, scaled with sensitivity
//! // get roll and pitch estimate - averaged accross n readings (steps) // let gyro = mpu.get_gyro()?;
//! let acc = mpu.get_acc_angles_avg(Steps(5))?; // println!("gyro: {:?}", gyro);
//! println!("r/p avg: {:?}", acc); //
//! // // get accelerometer data, scaled with sensitivity
//! // get temp // let acc = mpu.get_acc()?;
//! let temp = mpu.get_temp()?; // println!("acc: {:?}", acc);
//! println!("temp: {:?}c", temp); // }
//! // }
//! // get temp - averages across n readings (steps)
//! let temp = mpu.get_temp_avg(Steps(5))?;
//! println!("temp avg: {:?}c", temp);
//!
//! // get gyro data, scaled with sensitivity
//! let gyro = mpu.get_gyro()?;
//! println!("gyro: {:?}", gyro);
//!
//! // get gyro data, scaled with sensitivity - averaged across n readings (steps)
//! let gyro = mpu.get_gyro_avg(Steps(5))?;
//! println!("gyro avg: {:?}", gyro);
//!
//! // get accelerometer data, scaled with sensitivity
//! let acc = mpu.get_acc()?;
//! println!("acc: {:?}", acc);
//!
//! // get accelerometer data, scaled with sensitivity - averages across n readings (steps)
//! let acc = mpu.get_acc_avg(Steps(5))?;
//! println!("acc avg: {:?}", acc);
//! }
//!}
//! ``` //! ```
#![no_std] #![no_std]
@ -72,189 +51,18 @@ use embedded_hal::{
blocking::i2c::{Write, WriteRead}, blocking::i2c::{Write, WriteRead},
}; };
/// pi, taken straight from C /// PI, f32
pub const PI: f32 = 3.14159265358979323846; pub const PI: f32 = core::f32::consts::PI;
/// PI / 180, for conversion to radians
pub const PI_180: f32 = PI / 180.0;
/// Gyro Sensitivity /// Gyro Sensitivity
pub const FS_SEL: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4); pub const FS_SEL: (f32, f32, f32, f32) = (131., 65.5, 32.8, 16.4);
/// Accelerometer Sensitivity /// Accelerometer Sensitivity
pub const AFS_SEL: (f32, f32, f32, f32) = (16384., 8192., 4096., 2048.); pub const AFS_SEL: (f32, f32, f32, f32) = (16384., 8192., 4096., 2048.);
/// Operations trait for sensor readings
pub trait MutOps {
/// Add values to each readings fields
fn add(&mut self, operand: &Self);
/// Scales object fields with foo * 1/n
fn scale(&mut self, n: u8);
}
/// Helpers for sensor readings
pub trait Access<T> {
fn x(&self) -> T;
fn set_x(&mut self, val: T);
fn y(&self) -> T;
fn set_y(&mut self, val: T);
fn z(&self) -> T;
fn set_z(&mut self, val: T);
}
/// Trait for conversion from/to radians/degree
pub trait UnitConv<T> {
/// get radians from degree
fn to_rad(&self) -> T;
/// get radians from degree and change underlying data
fn to_rad_mut(&mut self);
/// get degree from radians
fn to_deg(&self) -> T;
/// get degree from radians and change underlying data
fn to_deg_mut(&mut self);
}
impl UnitConv<f32> for f32 {
fn to_rad(&self) -> f32 {
self * PI/180.0
}
fn to_rad_mut(&mut self) {
*self *= PI/180.0
}
fn to_deg(&self) -> f32 {
self * 180.0/PI
}
fn to_deg_mut(&mut self) {
*self *= 180.0/PI
}
}
impl Access<f32> for Vector3<f32> {
fn x(&self) -> f32 {
self[0]
}
fn set_x(&mut self, val: f32) {
self[0] = val;
}
fn y(&self) -> f32 {
self[1]
}
fn set_y(&mut self, val: f32) {
self[1] = val;
}
fn z(&self) -> f32 {
self[2]
}
fn set_z(&mut self, val: f32) {
self[2] = val
}
}
impl Access<f32> for Vector2<f32> {
fn x(&self) -> f32 {
self[0]
}
fn set_x(&mut self, val: f32) {
self[0] = val;
}
fn y(&self) -> f32 {
self[1]
}
fn set_y(&mut self, val: f32) {
self[1] = val;
}
fn z(&self) -> f32 {
-1.0
}
fn set_z(&mut self, _val: f32) {}
}
/// Used for bias calculation of chip in mpu::soft_calib
#[derive(Debug, Clone)]
pub struct Bias {
/// accelerometer axis bias
acc: Vector3<f32>,
/// gyro x axis bias
gyro: Vector3<f32>,
/// temperature AVERAGE: can't get bias!
temp: f32,
}
impl Default for Bias {
fn default() -> Bias {
Bias {
acc: Vector3::<f32>::zeros(),
gyro: Vector3::<f32>::zeros(),
temp: 0.0,
}
}
}
impl Bias {
fn add(&mut self, acc: Vector3<f32>, gyro: Vector3<f32>, temp: f32) {
self.acc += acc;
self.gyro += gyro;
self.temp += temp;
}
fn scale(&mut self, n: u8) {
let n = n as f32;
self.acc /= n;
self.gyro /= n;
self.temp /= n;
}
}
/// Reuse Bias struct for Variance calculation
pub type Variance = Bias;
impl Variance {
fn add_diff(&mut self, acc_diff: Vector3<f32>, gyro_diff: Vector3<f32>, temp_diff: f32) {
self.acc += acc_diff;
self.gyro += gyro_diff;
self.temp += temp_diff;
}
}
/// Vector2 for Roll/Pitch Reading
impl UnitConv<Vector2<f32>> for Vector2<f32> {
fn to_rad(&self) -> Vector2<f32> {
Vector2::<f32>::new(
self.x().to_rad(),
self.y().to_rad(),
)
}
fn to_rad_mut(&mut self) {
self[0].to_rad_mut();
self[1].to_rad_mut();
}
fn to_deg(&self) -> Vector2<f32> {
Vector2::<f32>::new(
self.x().to_deg(),
self.y().to_deg(),
)
}
fn to_deg_mut(&mut self) {
self[0].to_deg_mut();
self[1].to_deg_mut();
}
}
/// Helper struct used as number of steps for filtering
pub struct Steps(pub u8);
// Helper struct to convert Sensor measurement range to appropriate values defined in datasheet // Helper struct to convert Sensor measurement range to appropriate values defined in datasheet
struct Sensitivity(f32); struct Sensitivity(f32);
@ -311,8 +119,6 @@ pub enum Mpu6050Error<E> {
/// Handles all operations on/with Mpu6050 /// Handles all operations on/with Mpu6050
pub struct Mpu6050<I> { pub struct Mpu6050<I> {
i2c: I, i2c: I,
bias: Option<Bias>,
variance: Option<Variance>,
acc_sensitivity: f32, acc_sensitivity: f32,
gyro_sensitivity: f32, gyro_sensitivity: f32,
} }
@ -325,8 +131,6 @@ where
pub fn new(i2c: I) -> Self { pub fn new(i2c: I) -> Self {
Mpu6050 { Mpu6050 {
i2c, i2c,
bias: None,
variance: None,
acc_sensitivity: AFS_SEL.0, acc_sensitivity: AFS_SEL.0,
gyro_sensitivity: FS_SEL.0, gyro_sensitivity: FS_SEL.0,
} }
@ -336,8 +140,6 @@ where
pub fn new_with_sens(i2c: I, arange: AccelRange, grange: GyroRange) -> Self { pub fn new_with_sens(i2c: I, arange: AccelRange, grange: GyroRange) -> Self {
Mpu6050 { Mpu6050 {
i2c, i2c,
bias: None,
variance: None,
acc_sensitivity: Sensitivity::from(arange).0, acc_sensitivity: Sensitivity::from(arange).0,
gyro_sensitivity: Sensitivity::from(grange).0, gyro_sensitivity: Sensitivity::from(grange).0,
} }
@ -366,89 +168,15 @@ where
Ok(()) Ok(())
} }
/// Performs software calibration with steps number of readings
/// of accelerometer and gyrometer sensor
/// Readings must be made with MPU6050 in resting position
pub fn soft_calib(&mut self, steps: Steps) -> Result<(), Mpu6050Error<E>> {
let mut bias = Bias::default();
for _ in 0..steps.0+1 {
bias.add(self.get_acc()?, self.get_gyro()?, self.get_temp()?);
}
bias.scale(steps.0);
bias.acc[2] -= 1.0; // gravity compensation
self.bias = Some(bias);
Ok(())
}
/// Get bias of measurements
pub fn get_bias(&mut self) -> Option<&Bias> {
self.bias.as_ref()
}
/// Get variance of sensor by observing in resting state for steps
/// number of readings: accelerometer, gyro and temperature sensor each
pub fn calc_variance(&mut self, steps: Steps) -> Result<(), Mpu6050Error<E>> {
let iterations = steps.0;
if let None = self.bias {
self.soft_calib(steps)?;
}
let mut variance = Variance::default();
let mut acc = self.get_acc()?;
let mut gyro = self.get_gyro()?;
let mut temp = self.get_temp()?;
let mut acc_diff = Vector3::<f32>::zeros();
let mut gyro_diff = Vector3::<f32>::zeros();
let mut temp_diff: f32;
let bias = self.bias.clone().unwrap();
for _ in 0..iterations {
acc_diff.set_x(powf(acc.x() - bias.acc.x(), 2.0));
acc_diff.set_y(powf(acc.y() - bias.acc.y(), 2.0));
acc_diff.set_z(powf(acc.z() - bias.acc.z(), 2.0));
gyro_diff.set_x(powf(gyro.x() - bias.gyro.x(), 2.0));
gyro_diff.set_y(powf(gyro.y() - bias.gyro.y(), 2.0));
gyro_diff.set_z(powf(gyro.z() - bias.gyro.z(), 2.0));
temp_diff = powf(temp - bias.temp, 2.0);
variance.add_diff(acc_diff, gyro_diff, temp_diff);
acc = self.get_acc()?;
gyro = self.get_gyro()?;
temp = self.get_temp()?;
}
variance.scale(iterations-1);
variance.acc[2] -= 1.0; // gravity compensation
self.variance = Some(variance);
Ok(())
}
/// get variance of measurements
pub fn get_variance(&mut self) -> Option<&Variance> {
self.variance.as_ref()
}
/// Roll and pitch estimation from raw accelerometer readings /// Roll and pitch estimation from raw accelerometer readings
/// NOTE: no yaw! no magnetometer present on MPU6050 /// NOTE: no yaw! no magnetometer present on MPU6050
pub fn get_acc_angles(&mut self) -> Result<Vector2<f32>, Mpu6050Error<E>> { pub fn get_acc_angles(&mut self) -> Result<Vector2<f32>, Mpu6050Error<E>> {
let acc = self.get_acc()?; let acc = self.get_acc()?;
Ok(Vector2::<f32>::new(
atan2f(acc.y(), sqrtf(powf(acc.x(), 2.) + powf(acc.z(), 2.))),
atan2f(-acc.x(), sqrtf(powf(acc.y(), 2.) + powf(acc.z(), 2.)))
))
}
/// Roll and pitch estimation from raw accelerometer - averaged across window readings Ok(Vector2::<f32>::new(
pub fn get_acc_angles_avg(&mut self, steps: Steps) -> Result<Vector2<f32>, Mpu6050Error<E>> { atan2f(acc.y, sqrtf(powf(acc.x, 2.) + powf(acc.z, 2.))),
let mut acc = self.get_acc_angles()?; atan2f(-acc.x, sqrtf(powf(acc.y, 2.) + powf(acc.z, 2.)))
for _ in 0..steps.0-1 { ))
acc += self.get_acc_angles()?;
}
acc /= steps.0 as f32;
Ok(acc)
} }
/// Converts 2 bytes number in 2 compliment /// Converts 2 bytes number in 2 compliment
@ -480,23 +208,8 @@ where
/// Accelerometer readings in m/s^2 /// Accelerometer readings in m/s^2
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.addr())?;
acc /= self.acc_sensitivity; acc /= self.acc_sensitivity;
if let Some(ref bias) = self.bias {
acc -= bias.acc;
}
Ok(acc)
}
/// Accelerometer readings in m/s^2 - averaged
pub fn get_acc_avg(&mut self, steps: Steps) -> Result<Vector3<f32>, Mpu6050Error<E>> {
let mut acc = self.get_acc()?;
for _ in 0..steps.0-1 {
acc += self.get_acc()?;
}
acc /= steps.0 as f32;
Ok(acc) Ok(acc)
} }
@ -504,23 +217,8 @@ where
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.addr())?;
gyro *= PI / (180.0 * self.gyro_sensitivity); gyro *= PI_180 * self.gyro_sensitivity;
if let Some(ref bias) = self.bias {
gyro -= bias.gyro;
}
Ok(gyro)
}
/// Gyro readings in rad/s
pub fn get_gyro_avg(&mut self, steps: Steps) -> Result<Vector3<f32>, Mpu6050Error<E>> {
let mut gyro = self.get_gyro()?;
for _ in 0..steps.0-1 {
gyro += self.get_gyro()?;
}
gyro /= steps.0 as f32;
Ok(gyro) Ok(gyro)
} }
@ -533,20 +231,11 @@ where
Ok((raw_temp / 340.) + 36.53) Ok((raw_temp / 340.) + 36.53)
} }
pub fn get_temp_avg(&mut self, steps: Steps) -> Result<f32, Mpu6050Error<E>> {
let mut temp = self.get_temp()?;
for _ in 0..steps.0-1 {
temp += self.get_temp()?;
}
temp /= steps.0 as f32;
Ok(temp)
}
/// Writes byte to register /// Writes byte to register
pub fn write_u8(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6050Error<E>> { pub fn write_u8(&mut self, reg: u8, byte: u8) -> Result<(), Mpu6050Error<E>> {
self.i2c.write(SLAVE_ADDR.addr(), &[reg, byte]) self.i2c.write(SLAVE_ADDR.addr(), &[reg, byte])
.map_err(Mpu6050Error::I2c)?; .map_err(Mpu6050Error::I2c)?;
// delat disabled for dev build // delay disabled for dev build
// TODO: check effects with physical unit // TODO: check effects with physical unit
// self.delay.delay_ms(10u8); // self.delay.delay_ms(10u8);
Ok(()) Ok(())
@ -567,35 +256,3 @@ where
Ok(()) Ok(())
} }
} }
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_unit_conv() {
assert_eq!(1.0.to_rad(), 0.01745329252);
let mut deg = 1.0;
deg.to_rad_mut();
assert_eq!(deg, 0.01745329252);
assert_eq!(1.0.to_deg(), 57.295776);
let mut rad = 1.0;
rad.to_deg_mut();
assert_eq!(rad, 57.295776);
}
#[test]
fn test_nalgebra() {
let mut v = Vector3::<f32>::new(1., 1., 1.);
let o = v.clone();
v *= 3.;
assert_eq!(Vector3::<f32>::new(3., 3., 3.), v);
v /= 3.;
assert_eq!(o, v);
v -= o;
assert_eq!(Vector3::<f32>::new(0., 0., 0.), v);
}
}

6
src/registers.rs
View file

@ -7,11 +7,11 @@ pub enum Registers {
SLAVE_ADDR = 0x68, SLAVE_ADDR = 0x68,
/// Internal register to check slave addr /// Internal register to check slave addr
WHOAMI = 0x75, WHOAMI = 0x75,
/// High Bytle Register Gyro x orientation /// High Byte Register Gyro x orientation
GYRO_REGX_H = 0x43, GYRO_REGX_H = 0x43,
/// High Bytle Register Gyro y orientation /// High Byte Register Gyro y orientation
GYRO_REGY_H = 0x45, GYRO_REGY_H = 0x45,
/// High Bytle Register Gyro z orientation /// High Byte Register Gyro z orientation
GYRO_REGZ_H = 0x47, GYRO_REGZ_H = 0x47,
/// High Byte Register Calc roll /// High Byte Register Calc roll
ACC_REGX_H = 0x3b, ACC_REGX_H = 0x3b,