Improved Invensense stability by adding average of first few samples to better eliminate DC bias/drift

interface/src/SerialInterface.cpp

@@ -2,16 +2,7 @@
 //  SerialInterface.cpp
 //  2012 by Philip Rosedale for High Fidelity Inc.
 //
-//  Read interface data from the gyros/accelerometer board using SerialUSB
+//  Read interface data from the gyros/accelerometer Invensense board using the SerialUSB
-//
-//  Channels are received in the following order (integer 0-4096 based on voltage 0-3.3v)
-//
-//  0 - AIN 15:  Pitch Gyro (nodding your head 'yes')
-//  1 - AIN 16:  Yaw Gyro (shaking your head 'no')
-//  2 - AIN 17:  Roll Gyro (looking quizzical, tilting your head)
-//  3 - AIN 18:  Lateral acceleration (moving from side-to-side in front of your monitor)
-//  4 - AIN 19:  Up/Down acceleration (sitting up/ducking in front of your monitor)
-//  5 - AIN 20:  Forward/Back acceleration (Toward or away from your monitor)
 //
 
 #include "SerialInterface.h"
@@ -22,14 +13,8 @@
 #include <string>
 #endif
 
-const int MAX_BUFFER = 255;
-char serialBuffer[MAX_BUFFER];
-int serialBufferPos = 0;
-
-const int ZERO_OFFSET = 2048;
 const short NO_READ_MAXIMUM_MSECS = 3000;
-const short SAMPLES_TO_DISCARD = 100;               //  Throw out the first few samples
+const int GRAVITY_SAMPLES = 60;                     //  Use the first few samples to baseline values
-const int GRAVITY_SAMPLES = 60;                     //  Use the first samples to compute gravity vector
 
 const bool USING_INVENSENSE_MPU9150 = 1;
 
@@ -72,7 +57,8 @@ void SerialInterface::initializePort(char* portname, int baud) {
     if (_serialDescriptor == -1) {
         printLog("Failed.\n");
         return;
-    }    
+    }
+    
     struct termios options;
     tcgetattr(_serialDescriptor, &options);
     
@@ -101,6 +87,7 @@ void SerialInterface::initializePort(char* portname, int baud) {
     options.c_cflag |= CS8;
     tcsetattr(_serialDescriptor, TCSANOW, &options);
     
+    
     if (USING_INVENSENSE_MPU9150) {
         // block on invensense reads until there is data to read
         int currentFlags = fcntl(_serialDescriptor, F_GETFL);
@@ -140,11 +127,11 @@ void SerialInterface::renderLevels(int width, int height) {
         const int LEVEL_CORNER_Y = 200;
         
         // Draw the numeric degree/sec values from the gyros
-        sprintf(val, "Yaw   %4.1f", _lastYawRate);
+        sprintf(val, "Yaw   %4.1f", getLastYawRate());
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y, 0.10, 0, 1.0, 1, val, 0, 1, 0);
-        sprintf(val, "Pitch %4.1f", _lastPitchRate);
+        sprintf(val, "Pitch %4.1f", getLastPitchRate());
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0);
-        sprintf(val, "Roll  %4.1f", _lastRollRate);
+        sprintf(val, "Roll  %4.1f", getLastRollRate());
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0);
         sprintf(val, "X     %4.3f", _lastAccelX);
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 45, 0.10, 0, 1.0, 1, val, 0, 1, 0);
@@ -161,11 +148,11 @@ void SerialInterface::renderLevels(int width, int height) {
         glBegin(GL_LINES);
         // Gyro rates
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastYawRate, LEVEL_CORNER_Y - 3);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastYawRate(), LEVEL_CORNER_Y - 3);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastPitchRate, LEVEL_CORNER_Y + 12);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastPitchRate(), LEVEL_CORNER_Y + 12);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastRollRate, LEVEL_CORNER_Y + 27);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastRollRate(), LEVEL_CORNER_Y + 27);
         // Acceleration
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 42);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelX - _gravity.x)* ACCEL_VIEW_SCALING),
@@ -232,26 +219,33 @@ void SerialInterface::readData() {
         convertHexToInt(sensorBuffer + 30, pitchRate);
         
         //  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 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 + PITCH_BIAS;
+        _lastPitchRate = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
         
-        //  Accumulate an initial reading for gravity
+        //  Accumulate a set of initial baseline readings for setting gravity
-        //  Use a set of initial samples to compute gravity
+        if (totalSamples == 0) {
-        if (totalSamples < GRAVITY_SAMPLES) {
+            _averageGyroRates[0] = _lastRollRate;
-            _gravity.x += _lastAccelX;
+            _averageGyroRates[1] = _lastYawRate;
-            _gravity.y += _lastAccelY;
+            _averageGyroRates[2] = _lastPitchRate;
-            _gravity.z += _lastAccelZ;
+            _gravity.x = _lastAccelX;
-        }
+            _gravity.y = _lastAccelY;
-        if (totalSamples == GRAVITY_SAMPLES) {
+            _gravity.z = _lastAccelZ;
-            _gravity /= (float) totalSamples;
-            printLog("Gravity: %f\n", glm::length(_gravity));
-        }
 
+        }
+        else if (totalSamples < GRAVITY_SAMPLES) {
+            _gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity +
+            1.f/(float)GRAVITY_SAMPLES * glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ);
+            
+            _averageGyroRates[0] =  (1.f - 1.f/(float)GRAVITY_SAMPLES) * _averageGyroRates[0] +
+                                            1.f/(float)GRAVITY_SAMPLES * _lastRollRate;
+            _averageGyroRates[1] =  (1.f - 1.f/(float)GRAVITY_SAMPLES) * _averageGyroRates[1] +
+                                            1.f/(float)GRAVITY_SAMPLES * _lastYawRate;
+            _averageGyroRates[2] =  (1.f - 1.f/(float)GRAVITY_SAMPLES) * _averageGyroRates[2] +
+                                            1.f/(float)GRAVITY_SAMPLES * _lastPitchRate;
+        }
+        
         totalSamples++;
     } 
     
@@ -269,14 +263,17 @@ void SerialInterface::readData() {
 #endif
 }
 
-void SerialInterface::resetSerial() {
+void SerialInterface::resetAverages() {
-#ifdef __APPLE__
-    active = false;
     totalSamples = 0;
     _gravity = glm::vec3(0, 0, 0);
-    
+    _averageGyroRates = glm::vec3(0, 0, 0);
+}
+
+void SerialInterface::resetSerial() {
+#ifdef __APPLE__
+    resetAverages();
+    active = false;
     gettimeofday(&lastGoodRead, NULL);
-    
 #endif
 }
 

interface/src/SerialInterface.h

@@ -37,6 +37,8 @@ extern const bool USING_INVENSENSE_MPU9150;
 class SerialInterface {
 public:
     SerialInterface() : active(false),
+                        _gravity(0,0,0),
+                        _averageGyroRates(0,0,0),
                         _lastAccelX(0),
                         _lastAccelY(0),
                         _lastAccelZ(0),
@@ -47,13 +49,14 @@ public:
     void pair();
     void readData();
     
-    float getLastYawRate() const { return _lastYawRate; }
+    float getLastYawRate() const { return _lastYawRate - _averageGyroRates[1]; }
-    float getLastPitchRate() const { return _lastPitchRate; }
+    float getLastPitchRate() const { return _lastPitchRate - _averageGyroRates[2]; }
-    float getLastRollRate() const { return _lastRollRate; }
+    float getLastRollRate() const { return _lastRollRate - _averageGyroRates[0]; }
     glm::vec3 getLastAcceleration() { return glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ); };
     glm::vec3 getGravity() {return _gravity;};
     
     void renderLevels(int width, int height);
+    void resetAverages();
     bool active;
     
 private:
@@ -64,6 +67,7 @@ private:
     int totalSamples;
     timeval lastGoodRead;
     glm::vec3 _gravity;
+    glm::vec3 _averageGyroRates;
     float _lastAccelX;
     float _lastAccelY;
     float _lastAccelZ;

interface/src/main.cpp

@@ -363,12 +363,14 @@ void terminate () {
     exit(EXIT_SUCCESS);
 }
 
-void reset_sensors() {
+void resetSensors() {
     
     myAvatar.setPosition(start_location);
     headMouseX = ::screenWidth / 2;
     headMouseY = ::screenHeight / 2;
-    
+    if (serialPort.active) {
+        serialPort.resetAverages();
+    }
     myAvatar.reset();
 }
 
@@ -1717,7 +1719,7 @@ void key(unsigned char k, int x, int y) {
     if (k == 'c') myAvatar.setDriveKeys(DOWN, 1);
     if (k == 'w') myAvatar.setDriveKeys(FWD, 1);
     if (k == 's') myAvatar.setDriveKeys(BACK, 1);
-    if (k == ' ') reset_sensors();
+    if (k == ' ') resetSensors();
     if (k == 'a') myAvatar.setDriveKeys(ROT_LEFT, 1);
     if (k == 'd') myAvatar.setDriveKeys(ROT_RIGHT, 1);