From a9882d054f5b56cac3aa7911723f09854f644c8b Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philiprosedale@Philips-MacBook-Pro-2.local>
Date: Wed, 8 May 2013 14:39:41 -0700
Subject: [PATCH] Invensense gyros now correctly controlling head motion, code
 cleanups.

---
 interface/src/Avatar.cpp          | 50 ++++++++++++++++++-------------
 interface/src/SerialInterface.cpp |  5 +++-
 2 files changed, 34 insertions(+), 21 deletions(-)

diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 6f3ecd35b4..f63e0c9978 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -260,38 +260,38 @@ void Avatar::reset() {
     _head.leanForward = _head.leanSideways = 0;
 }
 
-// this pertains to moving the head with the glasses
-//  Using serial data, update avatar/render position and angles
+
+//  Update avatar head rotation with sensor data
 void Avatar::UpdateGyros(float frametime, SerialInterface* serialInterface, glm::vec3* gravity) {
-    float measured_pitch_rate = 0.0f;
-    float measured_roll_rate = 0.0f;
+    float measuredPitchRate = 0.0f;
+    float measuredRollRate = 0.0f;
+    float measuredYawRate = 0.0f;
+    
     if (serialInterface->active && USING_INVENSENSE_MPU9150) {
-        measured_pitch_rate = serialInterface->getLastPitchRate();
-        _head.yawRate = serialInterface->getLastYawRate();
-        measured_roll_rate = -1 * serialInterface->getLastRollRate();
+        measuredPitchRate = serialInterface->getLastPitchRate();
+        measuredYawRate = serialInterface->getLastYawRate();
+        measuredRollRate = serialInterface->getLastRollRate();
     } else {
-        measured_pitch_rate = serialInterface->getRelativeValue(HEAD_PITCH_RATE);
-        _head.yawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE);
-        measured_roll_rate = serialInterface->getRelativeValue(HEAD_ROLL_RATE);
+        measuredPitchRate = serialInterface->getRelativeValue(HEAD_PITCH_RATE);
+        measuredYawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE);
+        measuredRollRate = serialInterface->getRelativeValue(HEAD_ROLL_RATE);
     }
     
     //  Update avatar head position based on measured gyro rates
-    const float HEAD_ROTATION_SCALE = 0.70;
-    const float HEAD_ROLL_SCALE = 0.40;
     const float MAX_PITCH = 45;
     const float MIN_PITCH = -45;
     const float MAX_YAW = 85;
     const float MIN_YAW = -85;
+    const float MAX_ROLL = 50;
+    const float MIN_ROLL = -50;
     
-    if ((_headPitch < MAX_PITCH) && (_headPitch > MIN_PITCH)) {
-        addHeadPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime);
-    }
-        
-    addHeadRoll(measured_roll_rate * HEAD_ROLL_SCALE * frametime);
+    addHeadPitch(measuredPitchRate * frametime);
+    addHeadYaw(measuredYawRate * frametime);
+    addHeadRoll(measuredRollRate * frametime);
     
-    if ((_headYaw < MAX_YAW) && (_headYaw > MIN_YAW)) {
-         addHeadYaw(_head.yawRate * HEAD_ROTATION_SCALE * frametime);
-    }
+    setHeadPitch(glm::clamp(getHeadPitch(), MIN_PITCH, MAX_PITCH));
+    setHeadYaw(glm::clamp(getHeadYaw(), MIN_YAW, MAX_YAW));
+    setHeadRoll(glm::clamp(getHeadRoll(), MIN_ROLL, MAX_ROLL));
 }
 
 float Avatar::getAbsoluteHeadYaw() const {
@@ -518,6 +518,16 @@ void Avatar::updateHead(float deltaTime) {
         _headRoll  *= (1.0f - DECAY * _head.returnSpringScale * 2 * deltaTime);
     }
     
+    //  For invensense gyro, decay only slightly when roughly centered
+    if (USING_INVENSENSE_MPU9150) {
+        const float RETURN_RANGE = 5.0;
+        const float RETURN_STRENGTH = 1.0;
+        if (fabs(_headPitch) < RETURN_RANGE) { _headPitch *= (1.0f - RETURN_STRENGTH * deltaTime); }
+        if (fabs(_headYaw) < RETURN_RANGE) { _headYaw *= (1.0f - RETURN_STRENGTH * deltaTime); }
+        if (fabs(_headRoll) < RETURN_RANGE) { _headRoll *= (1.0f - RETURN_STRENGTH * deltaTime); }
+
+    }
+    
     if (_head.noise) {
         //  Move toward new target
         _headPitch += (_head.pitchTarget - _headPitch) * 10 * deltaTime; // (1.f - DECAY*deltaTime)*Pitch + ;
diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 9799d3104c..692069e4be 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -246,9 +246,12 @@ void SerialInterface::readData() {
         
         //  Convert the integer rates to floats
         const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f;     //  From MPU-9150 register map, 2000 deg/sec.
+        const float PITCH_BIAS = 2.0;                            //  Strangely, there is a small DC bias in the
+                                                                 //  invensense pitch reading.  Gravity?
+
         _lastRollRate = (float) rollRate * LSB_TO_DEGREES_PER_SECOND;
         _lastYawRate = (float) yawRate * LSB_TO_DEGREES_PER_SECOND;
-        _lastPitchRate = (float) pitchRate * LSB_TO_DEGREES_PER_SECOND;
+        _lastPitchRate = (float) -pitchRate * LSB_TO_DEGREES_PER_SECOND + PITCH_BIAS;
         
         totalSamples++;
     } else {