From 1674b6e659a2df3b9454ea97dedca6dc014ac809 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 23 Apr 2019 11:08:46 +0200 Subject: [PATCH 1/9] remove recorder -> moved to viz branch --- example/src/bin/main.rs | 31 ------------------- example/src/bin/recorder.rs | 62 ------------------------------------- 2 files changed, 93 deletions(-) delete mode 100644 example/src/bin/main.rs delete mode 100644 example/src/bin/recorder.rs diff --git a/example/src/bin/main.rs b/example/src/bin/main.rs deleted file mode 100644 index 170769c..0000000 --- a/example/src/bin/main.rs +++ /dev/null @@ -1,31 +0,0 @@ -use mpu6050::*; -use linux_embedded_hal::{I2cdev, Delay}; -use i2cdev::linux::LinuxI2CError; - -fn main() -> Result<(), Error> { - let i2c = I2cdev::new("/dev/i2c-1") - .map_err(Error::I2c)?; - - let delay = Delay; - - let mut mpu = Mpu6050::new(i2c, delay); - mpu.init()?; - - 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); - } -} diff --git a/example/src/bin/recorder.rs b/example/src/bin/recorder.rs deleted file mode 100644 index d44e7c8..0000000 --- a/example/src/bin/recorder.rs +++ /dev/null @@ -1,62 +0,0 @@ -use mpu6050::*; -use mpu6050::Error as Mpu6050Error; -use linux_embedded_hal::{I2cdev, Delay}; -use i2cdev::linux::LinuxI2CError; -use std::io::prelude::*; -use std::fs::File; -use std::path::Path; -use std::error::Error; - -fn new_file(name: &str) -> File { - let path = Path::new(name); - let display = path.display(); - let _file = match File::create(&path) { - Err(why) => panic!("couldn't create {}: {}", - display, - why.description()), - Ok(file) => return file, - }; -} - -fn write_x_to(file: &mut File, content: String) { - match file.write_all(content.as_bytes()) { - Err(why) => { - println!("couldn't write to file: {}", why.description()); - }, - Ok(_) => {}, - } -} - -fn main() -> Result<(), Mpu6050Error> { - let i2c = I2cdev::new("/dev/i2c-1") - .map_err(Mpu6050Error::I2c)?; - - let delay = Delay; - - let mut mpu = Mpu6050::new(i2c, delay); - mpu.init()?; - - - let mut acc_file = new_file("acc_data.txt"); - let mut gyro_file = new_file("gyro_data.txt"); - let mut temp_file = new_file("temp_data.txt"); - let mut angles_file = new_file("angles_data.txt"); - - loop { - // get roll and pitch estimate - let acc = mpu.get_acc_angles()?; - write_x_to(&mut angles_file, format!("{},{}", acc.0, acc.1)); - - // get temp - let temp = mpu.get_temp()?; - write_x_to(&mut temp_file, format!("{}", 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)); - - // get accelerometer data, scaled with sensitivity - let acc = mpu.get_acc()?; - write_x_to(&mut acc_file, format!("{} {} {}", acc.0, acc.1, acc.2)); - } -} From 577acd29a2a429c69bd699e478efc024c875f29a Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 23 Apr 2019 11:09:00 +0200 Subject: [PATCH 2/9] make example simpler --- example/src/main.rs | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 example/src/main.rs diff --git a/example/src/main.rs b/example/src/main.rs new file mode 100644 index 0000000..170769c --- /dev/null +++ b/example/src/main.rs @@ -0,0 +1,31 @@ +use mpu6050::*; +use linux_embedded_hal::{I2cdev, Delay}; +use i2cdev::linux::LinuxI2CError; + +fn main() -> Result<(), Error> { + let i2c = I2cdev::new("/dev/i2c-1") + .map_err(Error::I2c)?; + + let delay = Delay; + + let mut mpu = Mpu6050::new(i2c, delay); + mpu.init()?; + + 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); + } +} From 02cf4c5799d1eb04643ea9c376cd2ede9dd83515 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 23 Apr 2019 15:41:13 +0200 Subject: [PATCH 3/9] calculate variance from sensor data --- example/Cargo.toml | 2 +- example/src/main.rs | 9 ++++++-- src/lib.rs | 50 ++++++++++++++++++++++++++++++++++++++++++--- 3 files changed, 55 insertions(+), 6 deletions(-) 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..f7d8bc0 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().take().unwrap()); + println!("Calculated variance: {:?}", mpu.get_variance().take().unwrap()); loop { // get roll and pitch estimate diff --git a/src/lib.rs b/src/lib.rs index 0016cb2..d094721 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,4 +1,4 @@ -#![no_std] +//#![no_std] pub mod registers; @@ -14,8 +14,8 @@ use embedded_hal::{ }; /// Used for bias calculation of chip in mpu::soft_calib -#[derive(Default)] -struct Bias { +#[derive(Default, Debug)] +pub struct Bias { /// accelerometer x axis bias ax: f32, /// accelerometer y axis bias @@ -51,6 +51,8 @@ impl Bias { } } +pub type Variance = Bias; + // Helper struct to convert Sensor measurement range to appropriate values defined in datasheet struct Sensitivity(f32); @@ -109,6 +111,7 @@ pub struct Mpu6050 { i2c: I, delay: D, bias: Option, + variance: Option, acc_sensitivity: f32, gyro_sensitivity: f32, } @@ -124,6 +127,7 @@ where i2c, delay, bias: None, + variance: None, acc_sensitivity: AFS_SEL.0, gyro_sensitivity: FS_SEL.0, } @@ -135,6 +139,7 @@ where i2c, delay, bias: None, + variance: None, acc_sensitivity: Sensitivity::from(arange).0, gyro_sensitivity: Sensitivity::from(grange).0, } @@ -150,11 +155,50 @@ where } bias.scale(steps); + bias.az -= 1.0; // gravity compensation self.bias = Some(bias); Ok(()) } + pub fn get_bias(&mut self) -> &mut Option { + &mut self.bias + } + + /// Get variance of sensor by observing in resting state for steps + /// number of readings + pub fn calc_variance(&mut self, steps: u8) -> Result<(), Mpu6050Error> { + if let None = self.bias { + self.soft_calib(100)?; + } + + let mut variance = Variance::default(); + let mut acc = self.get_acc()?; + let mut gyro = self.get_gyro()?; + let mut acc_diff: (f32, f32, f32); + let mut gyro_diff: (f32, f32, f32); + let bias = &self.bias.take().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)); + variance.add(acc_diff, gyro_diff); + acc = self.get_acc()?; + gyro = self.get_gyro()?; + } + + variance.scale(steps-1); + variance.az -= 1.0; // gravity compensation + self.variance = Some(variance); + println!("{:?}", self.variance.take().unwrap()); + + Ok(()) + } + + pub fn get_variance(&mut self) -> &mut Option { + &mut self.variance + } + /// Wakes MPU6050 with all sensors enabled (default) pub fn wake(&mut self) -> Result<(), Mpu6050Error> { self.write_u8(POWER_MGMT_1, 0)?; From f423a740db7ec4636ef3a20d0b4a9ab15bad3143 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 23 Apr 2019 16:28:28 +0200 Subject: [PATCH 4/9] calculate variance from sensor data --- src/lib.rs | 40 +++++++++++++++++++++++++--------------- 1 file changed, 25 insertions(+), 15 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index d094721..6c2300f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,4 +1,4 @@ -//#![no_std] +#![no_std] pub mod registers; @@ -14,7 +14,7 @@ use embedded_hal::{ }; /// Used for bias calculation of chip in mpu::soft_calib -#[derive(Default, Debug)] +#[derive(Default, Debug, Clone)] pub struct Bias { /// accelerometer x axis bias ax: f32, @@ -28,16 +28,19 @@ pub 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,6 +51,7 @@ impl Bias { self.gx /= n; self.gy /= n; self.gz /= n; + self.t /= n; } } @@ -145,13 +149,14 @@ where } } - /// Performs software calibration with steps number of readings. + /// 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()?); + bias.add(self.get_acc()?, self.get_gyro()?, self.get_temp()?); } bias.scale(steps); @@ -160,13 +165,14 @@ where Ok(()) } - - pub fn get_bias(&mut self) -> &mut Option { - &mut self.bias + + /// 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 + /// 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(100)?; @@ -175,28 +181,32 @@ where 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 bias = &self.bias.take().unwrap(); + 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)); - variance.add(acc_diff, gyro_diff); + 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); - println!("{:?}", self.variance.take().unwrap()); - + Ok(()) } - pub fn get_variance(&mut self) -> &mut Option { - &mut self.variance + /// get variance of measurements + pub fn get_variance(&mut self) -> Option<&Variance> { + self.variance.as_ref() } /// Wakes MPU6050 with all sensors enabled (default) From 5398f1d47232fe19fd27e1f53051567b2ba937fc Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 23 Apr 2019 16:28:46 +0200 Subject: [PATCH 5/9] update example --- example/src/main.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/example/src/main.rs b/example/src/main.rs index f7d8bc0..16243df 100644 --- a/example/src/main.rs +++ b/example/src/main.rs @@ -13,8 +13,8 @@ fn main() -> Result<(), Mpu6050Error> { mpu.soft_calib(100)?; mpu.calc_variance(50)?; - println!("Calibrated with bias: {:?}", mpu.get_bias().take().unwrap()); - println!("Calculated variance: {:?}", mpu.get_variance().take().unwrap()); + println!("Calibrated with bias: {:?}", mpu.get_bias().unwrap()); + println!("Calculated variance: {:?}", mpu.get_variance().unwrap()); loop { // get roll and pitch estimate From 6079fb4cd043686dbb2cceb9f5e4e8dd6a96fac4 Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Tue, 23 Apr 2019 16:31:02 +0200 Subject: [PATCH 6/9] update example --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index 7303311..dcd18a7 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 From 4a277b1ee166e404e26e72866d7f09a005bf47f4 Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 23 Apr 2019 18:24:02 +0200 Subject: [PATCH 7/9] restructure functions to better reflect typical usage --- src/lib.rs | 48 ++++++++++++++++++++++++------------------------ 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 6c2300f..4e73fcb 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -149,6 +149,29 @@ where } } + /// Wakes MPU6050 with all sensors enabled (default) + pub fn wake(&mut self) -> Result<(), Mpu6050Error> { + self.write_u8(POWER_MGMT_1, 0)?; + self.delay.delay_ms(100u8); + Ok(()) + } + + /// Init wakes MPU6050 and verifies register addr, e.g. in i2c + pub fn init(&mut self) -> Result<(), Mpu6050Error> { + self.wake()?; + self.verify()?; + Ok(()) + } + + /// Verifies device to address 0x68 with WHOAMI Register + pub fn verify(&mut self) -> Result<(), Mpu6050Error> { + let address = self.read_u8(WHOAMI)?; + if address != SLAVE_ADDR { + return Err(Mpu6050Error::InvalidChipId(address)); + } + Ok(()) + } + /// Performs software calibration with steps number of readings /// of accelerometer and gyrometer sensor /// Readings must be made with MPU6050 in resting position @@ -175,7 +198,7 @@ where /// 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(100)?; + self.soft_calib(steps)?; } let mut variance = Variance::default(); @@ -209,29 +232,6 @@ where self.variance.as_ref() } - /// Wakes MPU6050 with all sensors enabled (default) - pub fn wake(&mut self) -> Result<(), Mpu6050Error> { - self.write_u8(POWER_MGMT_1, 0)?; - self.delay.delay_ms(100u8); - Ok(()) - } - - /// Init wakes MPU6050 and verifies register addr, e.g. in i2c - pub fn init(&mut self) -> Result<(), Mpu6050Error> { - self.wake()?; - self.verify()?; - Ok(()) - } - - /// Verifies device to address 0x68 with WHOAMI Register - pub fn verify(&mut self) -> Result<(), Mpu6050Error> { - let address = self.read_u8(WHOAMI)?; - if address != SLAVE_ADDR { - return Err(Mpu6050Error::InvalidChipId(address)); - } - Ok(()) - } - /// 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> { From e9da624fde17b905fb646f71491b5bab9d1cc41e Mon Sep 17 00:00:00 2001 From: Julian Gaal Date: Tue, 23 Apr 2019 18:26:08 +0200 Subject: [PATCH 8/9] Update TODO --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index dcd18a7..c2a03c9 100644 --- a/README.md +++ b/README.md @@ -78,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 From 4c5fd4545cc6854383072a2174923ff03f7278ab Mon Sep 17 00:00:00 2001 From: juliangaal Date: Tue, 23 Apr 2019 18:28:49 +0200 Subject: [PATCH 9/9] typo --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index dcd18a7..b5661e8 100644 --- a/README.md +++ b/README.md @@ -48,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/)