diff --git a/README.md b/README.md index 7303311..d531cff 100644 --- a/README.md +++ b/README.md @@ -23,6 +23,11 @@ fn main() -> Result<(), Error> { let mut mpu = Mpu6050::new(i2c, delay); mpu.init()?; + mpu.soft_calib(100)?; + mpu.calc_variance(50)?; + + println!("Calibrated with bias: {:?}", mpu.get_bias().unwrap()); + println!("Calculated variance: {:?}", mpu.get_variance().unwrap()); loop { // get roll and pitch estimate @@ -43,7 +48,7 @@ fn main() -> Result<(), Error> { } } ``` -*Note*: this example uses API of version published on crates.io, not local master branch. +*Note*: this example uses API of version on local master branch. Some functions may not be available on published crate yet. #### Compile linux example (Raspberry Pi 3B) files [here](https://github.com/juliangaal/mpu6050/blob/master/example/) @@ -73,10 +78,11 @@ $ cargo build --target=armv7-unknown-linux-gnueabihf - [x] read gyro data - [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 +- [ ] complementary filter for roll, pitch estimate, possible on device? - [ ] 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 diff --git a/example/Cargo.toml b/example/Cargo.toml index c5b777b..76a2d21 100644 --- a/example/Cargo.toml +++ b/example/Cargo.toml @@ -5,6 +5,6 @@ authors = ["Julian Gaal "] edition = "2018" [dependencies] -mpu6050 = "0.1.0" +mpu6050 = { path = "../" } i2cdev = "0.4.1" linux-embedded-hal = "0.2.2" diff --git a/example/src/main.rs b/example/src/main.rs index 170769c..16243df 100644 --- a/example/src/main.rs +++ b/example/src/main.rs @@ -2,14 +2,19 @@ use mpu6050::*; use linux_embedded_hal::{I2cdev, Delay}; use i2cdev::linux::LinuxI2CError; -fn main() -> Result<(), Error> { +fn main() -> Result<(), Mpu6050Error> { let i2c = I2cdev::new("/dev/i2c-1") - .map_err(Error::I2c)?; + .map_err(Mpu6050Error::I2c)?; let delay = Delay; let mut mpu = Mpu6050::new(i2c, delay); mpu.init()?; + mpu.soft_calib(100)?; + mpu.calc_variance(50)?; + + println!("Calibrated with bias: {:?}", mpu.get_bias().unwrap()); + println!("Calculated variance: {:?}", mpu.get_variance().unwrap()); loop { // get roll and pitch estimate diff --git a/src/lib.rs b/src/lib.rs index 0016cb2..4e73fcb 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -14,8 +14,8 @@ use embedded_hal::{ }; /// Used for bias calculation of chip in mpu::soft_calib -#[derive(Default)] -struct Bias { +#[derive(Default, Debug, Clone)] +pub struct Bias { /// accelerometer x axis bias ax: f32, /// accelerometer y axis bias @@ -28,16 +28,19 @@ struct Bias { gy: f32, /// gyro z axis bias gz: f32, + /// temperature AVERAGE: can't get bias! + t: f32, } impl Bias { - fn add(&mut self, acc: (f32, f32, f32), gyro: (f32, f32, f32)) { + fn add(&mut self, acc: (f32, f32, f32), gyro: (f32, f32, f32), temp: f32) { self.ax += acc.0; self.ay += acc.1; self.az += acc.2; self.gx += gyro.0; self.gy += gyro.1; self.gz += gyro.2; + self.t += temp; } fn scale(&mut self, n: u8) { @@ -48,9 +51,12 @@ impl Bias { self.gx /= n; self.gy /= n; self.gz /= n; + self.t /= n; } } +pub type Variance = Bias; + // Helper struct to convert Sensor measurement range to appropriate values defined in datasheet struct Sensitivity(f32); @@ -109,6 +115,7 @@ pub struct Mpu6050 { i2c: I, delay: D, bias: Option, + variance: Option, acc_sensitivity: f32, gyro_sensitivity: f32, } @@ -124,6 +131,7 @@ where i2c, delay, bias: None, + variance: None, acc_sensitivity: AFS_SEL.0, gyro_sensitivity: FS_SEL.0, } @@ -135,26 +143,12 @@ where i2c, delay, bias: None, + variance: None, acc_sensitivity: Sensitivity::from(arange).0, gyro_sensitivity: Sensitivity::from(grange).0, } } - /// Performs software calibration with steps number of readings. - /// Readings must be made with MPU6050 in resting position - pub fn soft_calib(&mut self, steps: u8) -> Result<(), Mpu6050Error> { - let mut bias = Bias::default(); - - for _ in 0..steps+1 { - bias.add(self.get_acc()?, self.get_gyro()?); - } - - bias.scale(steps); - self.bias = Some(bias); - - Ok(()) - } - /// Wakes MPU6050 with all sensors enabled (default) pub fn wake(&mut self) -> Result<(), Mpu6050Error> { self.write_u8(POWER_MGMT_1, 0)?; @@ -178,6 +172,66 @@ where 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: u8) -> Result<(), Mpu6050Error> { + let mut bias = Bias::default(); + + for _ in 0..steps+1 { + bias.add(self.get_acc()?, self.get_gyro()?, self.get_temp()?); + } + + bias.scale(steps); + bias.az -= 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: u8) -> Result<(), Mpu6050Error> { + 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: (f32, f32, f32); + let mut gyro_diff: (f32, f32, f32); + let mut temp_diff: f32; + let bias = self.bias.clone().unwrap(); + + for _ in 0..steps { + acc_diff = (powf(acc.0 - bias.ax, 2.0), powf(acc.1 - bias.ay, 2.0), powf(acc.2 - bias.az, 2.0)); + gyro_diff = (powf(gyro.0 - bias.gx, 2.0), powf(gyro.1 - bias.gy, 2.0), powf(gyro.2 - bias.gz, 2.0)); + temp_diff = powf(temp - bias.t, 2.0); + variance.add(acc_diff, gyro_diff, temp_diff); + acc = self.get_acc()?; + gyro = self.get_gyro()?; + temp = self.get_temp()?; + } + + variance.scale(steps-1); + variance.az -= 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 /// NOTE: no yaw! no magnetometer present on MPU6050 pub fn get_acc_angles(&mut self) -> Result<(f32, f32), Mpu6050Error> { diff --git a/viz/viz/src/bin/recorder.rs b/viz/viz/src/bin/recorder.rs index d44e7c8..f2ebf27 100644 --- a/viz/viz/src/bin/recorder.rs +++ b/viz/viz/src/bin/recorder.rs @@ -1,5 +1,4 @@ use mpu6050::*; -use mpu6050::Error as Mpu6050Error; use linux_embedded_hal::{I2cdev, Delay}; use i2cdev::linux::LinuxI2CError; use std::io::prelude::*; @@ -35,7 +34,10 @@ fn main() -> Result<(), Mpu6050Error> { let mut mpu = Mpu6050::new(i2c, delay); mpu.init()?; + mpu.soft_calib(200)?; + mpu.calc_variance(200)?; + println!("Calculated variance: {:?}", mpu.get_variance().unwrap()); let mut acc_file = new_file("acc_data.txt"); let mut gyro_file = new_file("gyro_data.txt"); @@ -45,18 +47,18 @@ fn main() -> Result<(), Mpu6050Error> { loop { // get roll and pitch estimate let acc = mpu.get_acc_angles()?; - write_x_to(&mut angles_file, format!("{},{}", acc.0, acc.1)); + write_x_to(&mut angles_file, format!("{},{}\n", acc.0, acc.1)); // get temp let temp = mpu.get_temp()?; - write_x_to(&mut temp_file, format!("{}", temp)); + write_x_to(&mut temp_file, format!("{}\n", temp)); // get gyro data, scaled with sensitivity let gyro = mpu.get_gyro()?; - write_x_to(&mut gyro_file, format!("{} {} {}", gyro.0, gyro.1, gyro.2)); - + write_x_to(&mut gyro_file, format!("{},{},{}\n", gyro.0, gyro.1, gyro.2)); + // get accelerometer data, scaled with sensitivity let acc = mpu.get_acc()?; - write_x_to(&mut acc_file, format!("{} {} {}", acc.0, acc.1, acc.2)); + write_x_to(&mut acc_file, format!("{},{},{}\n", acc.0, acc.1, acc.2)); } }