From 02cf4c5799d1eb04643ea9c376cd2ede9dd83515 Mon Sep 17 00:00:00 2001
From: juliangaal <juliangaal@protonmail.com>
Date: Tue, 23 Apr 2019 15:41:13 +0200
Subject: [PATCH] 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 <juliangaal@protonmail.com>"]
 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<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().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<I, D> {
     i2c: I,
     delay: D,
     bias: Option<Bias>,
+    variance: Option<Variance>,
     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<Bias> {
+        &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<E>> {
+        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<Variance> {
+        &mut self.variance
+    }
+
     /// Wakes MPU6050 with all sensors enabled (default)
     pub fn wake(&mut self) -> Result<(), Mpu6050Error<E>> {
         self.write_u8(POWER_MGMT_1, 0)?;