From 87dfbf9f4fe7ad6cdc0c1339f5e4c7eb98e83df3 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Mon, 13 May 2013 18:04:21 -0700 Subject: [PATCH] Improved Invensense stability by adding average of first few samples to better eliminate DC bias/drift --- interface/src/SerialInterface.cpp | 87 +++++++++++++++---------------- interface/src/SerialInterface.h | 10 ++-- interface/src/main.cpp | 8 +-- 3 files changed, 54 insertions(+), 51 deletions(-) diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 7aea1a46d9..961115df81 100644 --- a/interface/src/SerialInterface.cpp +++ b/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 -// -// 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) +// Read interface data from the gyros/accelerometer Invensense board using the SerialUSB // #include "SerialInterface.h" @@ -22,14 +13,8 @@ #include #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 samples to compute gravity vector +const int GRAVITY_SAMPLES = 60; // Use the first few samples to baseline values 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 PITCH_BIAS = 2.0; // Strangely, there is a small DC bias in the - // invensense pitch reading. Gravity? - + const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec. _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 - // Use a set of initial samples to compute gravity - if (totalSamples < GRAVITY_SAMPLES) { - _gravity.x += _lastAccelX; - _gravity.y += _lastAccelY; - _gravity.z += _lastAccelZ; - } - if (totalSamples == GRAVITY_SAMPLES) { - _gravity /= (float) totalSamples; - printLog("Gravity: %f\n", glm::length(_gravity)); - } + // Accumulate a set of initial baseline readings for setting gravity + if (totalSamples == 0) { + _averageGyroRates[0] = _lastRollRate; + _averageGyroRates[1] = _lastYawRate; + _averageGyroRates[2] = _lastPitchRate; + _gravity.x = _lastAccelX; + _gravity.y = _lastAccelY; + _gravity.z = _lastAccelZ; + } + 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() { -#ifdef __APPLE__ - active = false; +void SerialInterface::resetAverages() { 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 } diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h index 65a802d0c2..f8e8798f0c 100644 --- a/interface/src/SerialInterface.h +++ b/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 getLastPitchRate() const { return _lastPitchRate; } - float getLastRollRate() const { return _lastRollRate; } + float getLastYawRate() const { return _lastYawRate - _averageGyroRates[1]; } + float getLastPitchRate() const { return _lastPitchRate - _averageGyroRates[2]; } + 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; diff --git a/interface/src/main.cpp b/interface/src/main.cpp index 3b665f1455..b78722fce0 100644 --- a/interface/src/main.cpp +++ b/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);