From 090df81afebb29d8c95a2141cab728f9476f3d22 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 11 Jun 2013 15:58:50 -0700
Subject: [PATCH] Fixes to align better with previous code.

---
 interface/src/SerialInterface.cpp | 16 +++++++++-------
 interface/src/SerialInterface.h   |  7 ++++---
 2 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 02c672ba8f..86755e28b5 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -232,15 +232,15 @@ void SerialInterface::readData(float deltaTime) {
         rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
         rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
 
+        // update and subtract the long term average
+        _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
+                1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates;
+        rotationRates -= _averageRotationRates;
+
         // compute the angular acceleration
         glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
         _lastRotationRates = rotationRates;
-
-        _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
-                1.f/(float)LONG_TERM_RATE_SAMPLES * _lastRotationRates;
         
-        printLog("r: %g %g %g\n", _averageRotationRates.x, _averageRotationRates.y, _averageRotationRates.z);
-
         //  Update raw rotation estimates
         glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
             glm::quat(glm::radians(deltaTime * _lastRotationRates));
@@ -249,10 +249,10 @@ void SerialInterface::readData(float deltaTime) {
         _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
             _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
         
+        // update and subtract the long term average
         _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
                 1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
-        
-        printLog("a: %g %g %g\n", _averageAcceleration.x, _averageAcceleration.y, _averageAcceleration.z);
+        _estimatedAcceleration -= _averageAcceleration;
         
         //  Consider updating our angular velocity/acceleration to linear acceleration mapping
         if (glm::length(_estimatedAcceleration) > EPSILON &&
@@ -347,6 +347,8 @@ void SerialInterface::readData(float deltaTime) {
 void SerialInterface::resetAverages() {
     totalSamples = 0;
     _gravity = glm::vec3(0, 0, 0);
+    _averageRotationRates = glm::vec3(0, 0, 0);
+    _averageAcceleration = glm::vec3(0, 0, 0);
     _lastRotationRates = glm::vec3(0, 0, 0);
     _estimatedRotation = glm::vec3(0, 0, 0);
     _estimatedPosition = glm::vec3(0, 0, 0);
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index 88af60ccbe..8c918e65ff 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -26,6 +26,8 @@ class SerialInterface {
 public:
     SerialInterface() : active(false),
                         _gravity(0, 0, 0),
+                        _averageRotationRates(0, 0, 0),
+                        _averageAcceleration(0, 0, 0),
                         _estimatedRotation(0, 0, 0),
                         _estimatedPosition(0, 0, 0),
                         _estimatedVelocity(0, 0, 0),
@@ -66,6 +68,8 @@ private:
     int totalSamples;
     timeval lastGoodRead;
     glm::vec3 _gravity;
+    glm::vec3 _averageRotationRates;
+    glm::vec3 _averageAcceleration;
     glm::vec3 _estimatedRotation;
     glm::vec3 _estimatedPosition;
     glm::vec3 _estimatedVelocity;
@@ -73,9 +77,6 @@ private:
     glm::vec3 _lastAcceleration;
     glm::vec3 _lastRotationRates;
     
-    glm::vec3 _averageAcceleration;
-    glm::vec3 _averageRotationRates;
-    
     glm::mat3 _angularVelocityToLinearAccel;
     glm::mat3 _angularAccelToLinearAccel;
 };