diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 37b8cf31b6..7652c60461 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -884,7 +884,7 @@ void Application::idle() { // Read serial port interface devices if (_serialPort.active) { - _serialPort.readData(); + _serialPort.readData(deltaTime); } // Sample hardware, update view frustum if needed, and send avatar data to mixer/agents diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 1989736bf8..b730d2a907 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -15,6 +15,7 @@ const short NO_READ_MAXIMUM_MSECS = 3000; const int GRAVITY_SAMPLES = 60; // Use the first few samples to baseline values +const int LONG_TERM_RATE_SAMPLES = 1000; const bool USING_INVENSENSE_MPU9150 = 1; @@ -136,6 +137,16 @@ void SerialInterface::renderLevels(int width, int height) { 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 + getLastRollRate(), LEVEL_CORNER_Y + 27); + // Gyro Estimated Rotation + glColor4f(0, 1, 1, 1); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 1); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.y, LEVEL_CORNER_Y - 1); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 14); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.z, LEVEL_CORNER_Y + 14); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 29); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.x, LEVEL_CORNER_Y + 29); + + // Acceleration glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 42); glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelX - _gravity.x)* ACCEL_VIEW_SCALING), @@ -169,7 +180,7 @@ void convertHexToInt(unsigned char* sourceBuffer, int& destinationInt) { destinationInt = result; } -void SerialInterface::readData() { +void SerialInterface::readData(float deltaTime) { #ifdef __APPLE__ int initialSamples = totalSamples; @@ -207,6 +218,11 @@ void SerialInterface::readData() { _lastYawRate = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND; _lastPitchRate = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND; + // Update raw rotation estimates + _estimatedRotation += deltaTime * glm::vec3(_lastRollRate - _averageGyroRates[0], + _lastYawRate - _averageGyroRates[1], + _lastPitchRate - _averageGyroRates[2]); + // Accumulate a set of initial baseline readings for setting gravity if (totalSamples == 0) { _averageGyroRates[0] = _lastRollRate; @@ -216,17 +232,20 @@ void SerialInterface::readData() { _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; + } + else { + // Cumulate long term average to (hopefully) take DC bias out of rotation rates + _averageGyroRates[0] = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[0] + + 1.f/(float)LONG_TERM_RATE_SAMPLES * _lastRollRate; + _averageGyroRates[1] = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[1] + + 1.f/(float)LONG_TERM_RATE_SAMPLES * _lastYawRate; + _averageGyroRates[2] = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[2] + + 1.f/(float)LONG_TERM_RATE_SAMPLES * _lastPitchRate; + + if (totalSamples < GRAVITY_SAMPLES) { + _gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity + + 1.f/(float)GRAVITY_SAMPLES * glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ); + } } totalSamples++; diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h index 9aa7bccf04..42d24367f6 100644 --- a/interface/src/SerialInterface.h +++ b/interface/src/SerialInterface.h @@ -38,7 +38,8 @@ class SerialInterface { public: SerialInterface() : active(false), _gravity(0,0,0), - _averageGyroRates(0,0,0), + _averageGyroRates(0, 0, 0), + _estimatedRotation(0, 0, 0), _lastAccelX(0), _lastAccelY(0), _lastAccelZ(0), @@ -47,7 +48,7 @@ public: _lastRollRate(0) {} void pair(); - void readData(); + void readData(float deltaTime); float getLastYawRate() const { return _lastYawRate - _averageGyroRates[1]; } float getLastPitchRate() const { return _lastPitchRate - _averageGyroRates[2]; } @@ -68,6 +69,7 @@ private: timeval lastGoodRead; glm::vec3 _gravity; glm::vec3 _averageGyroRates; + glm::vec3 _estimatedRotation; float _lastAccelX; float _lastAccelY; float _lastAccelZ;