Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-05 00:59:56 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Long term averaging of the gyros for better stability

Browse source
This commit is contained in:
Philip Rosedale 2013-05-24 11:33:00 -07:00
parent 59ac5ebd66
commit 17c898c151
3 changed files with 36 additions and 15 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
interface/src/Application.cpp
View file

@ -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

43
interface/src/SerialInterface.cpp
View file

@ -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++;

6
interface/src/SerialInterface.h
View file

@ -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;