merge

README.md

@@ -23,6 +23,11 @@ fn main() -> Result<(), Error<LinuxI2CError>> {
 
     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<LinuxI2CError>> {
     }
 }
 ```
-*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

example/Cargo.toml

@@ -5,6 +5,6 @@ authors = ["Julian Gaal <juliangaal@protonmail.com>"]
 edition = "2018"
 
 [dependencies]
-mpu6050 = "0.1.0"
+mpu6050 = { path = "../" } 
 i2cdev = "0.4.1"
 linux-embedded-hal = "0.2.2"

example/src/main.rs

@@ -2,14 +2,19 @@ use mpu6050::*;
 use linux_embedded_hal::{I2cdev, Delay};
 use i2cdev::linux::LinuxI2CError;
 
-fn main() -> Result<(), Error<LinuxI2CError>> {
+fn main() -> Result<(), Mpu6050Error<LinuxI2CError>> {
     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

src/lib.rs

@@ -14,8 +14,8 @@ use embedded_hal::{
 };
 
 /// Used for bias calculation of chip in mpu::soft_calib
-#[derive(Default)]
+#[derive(Default, Debug, Clone)]
-struct Bias {
+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<I, D> {
     i2c: I,
     delay: D,
     bias: Option<Bias>,
+    variance: Option<Variance>,
     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<E>> {
-        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<E>> {
         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<E>> {
+        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<E>> {
+        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<E>> {

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<LinuxI2CError>> {
 
     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<LinuxI2CError>> {
     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));
     }
 }