From 1f76432fa1a0b0c369f3661ad51b9e68cf662194 Mon Sep 17 00:00:00 2001
From: Julian Gaal <juliangaal@protonmail.com>
Date: Mon, 15 Apr 2019 22:14:45 +0200
Subject: [PATCH] roll pitch estimation accelerometer alone

---
 Cargo.toml       |  1 +
 README.md        |  3 +++
 src/bin/linux.rs |  4 ++--
 src/lib.rs       | 18 +++++++++++-------
 4 files changed, 17 insertions(+), 9 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index cfadaa1..1afcfd2 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -8,3 +8,4 @@ edition = "2018"
 i2cdev = "0.4.1"
 embedded-hal = "0.2.2"
 linux-embedded-hal = "0.2.2"
+libm = "0.1.2"
diff --git a/README.md b/README.md
index 48f7833..b6c31d6 100644
--- a/README.md
+++ b/README.md
@@ -13,6 +13,9 @@ cross-compile with `cargo build --bin main --target=arm-unknown-linux-gnueabihf`
 - [ ] basic feature support as described [here](https://github.com/Tijndagamer/mpu6050/blob/master/mpu6050/mpu6050.py)
 - [x] read gyro data
 - [x] read acc data
+- [x] software calibration
+- [x] roll, pitch estimation accelerometer only
+- [ ] complementary filter for roll, pitch estimate
 - [ ] read temp data
 - [ ] 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)
diff --git a/src/bin/linux.rs b/src/bin/linux.rs
index 30e7905..0a91276 100644
--- a/src/bin/linux.rs
+++ b/src/bin/linux.rs
@@ -10,10 +10,10 @@ fn main() -> Result<(), Error<LinuxI2CError>> {
 
     let mut mpu = Mpu6050::new(i2c, delay);
     mpu.init()?;
-    mpu.soft_calibrate(200)?;
+    //mpu.soft_calibrate(200)?;
 
     loop {
-        match mpu.get_gyro() {
+        match mpu.get_acc_angles() {
             Ok(r) => {
                 println!("{:?}", r);
             },
diff --git a/src/lib.rs b/src/lib.rs
index 939010b..bb41d7e 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,15 +1,17 @@
 #![no_std]
 
-pub mod constants;
-
 ///! Mpu6050 sensor driver.
 ///! Datasheet: 
+
+pub mod constants;
+
+use crate::constants::*;
+use libm as m;
 use embedded_hal::{
     blocking::delay::DelayMs,
     blocking::i2c::{Read, Write, WriteRead},
 };
 
-use crate::constants::*;
 
 #[derive(Default)]
 struct Bias {
@@ -85,7 +87,7 @@ where
         }
     }
 
-    pub fn soft_calibrate(&mut self, steps: u8) -> Result<(), Error<E>> {
+    pub fn soft_calib(&mut self, steps: u8) -> Result<(), Error<E>> {
         let mut bias = Bias::default();
 
         for _ in 0..steps+1 {
@@ -118,10 +120,12 @@ where
         Ok(())
     }
 
-    /// Implements complementary filter for roll/pitch
     /// NOTE: yaw will always point up, sensor has no magnetometer to allow fusion
-    pub fn rpy(&mut self) -> Result<(f32, f32, f32), Error<E>> {
-        Ok((0.0, 0.0, 0.0))
+    pub fn get_acc_angles(&mut self) -> Result<(f32, f32), Error<E>> {
+        let (ax, ay, az) = self.get_acc()?;
+        let roll: f32 = m::atan2f(ay, m::sqrtf(m::powf(ax, 2.) + m::powf(az, 2.)));
+        let pitch: f32 = m::atan2f(-ax, m::sqrtf(m::powf(ay, 2.) + m::powf(az, 2.)));
+        Ok((roll, pitch))
     }
 
     // TODO work on removing unnecessary type conversion