Mirrors/overte-HifiExperiments
3
0
Fork 0
mirror of https://github.com/HifiExperiments/overte.git synced 2025-08-14 13:49:11 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge pull request #499 from ey6es/gyroquat

Browse source 
Converted gyro rotation computations to use quaternions, fixed coordinates and increased rate of convergence to current gravity.
This commit is contained in:
Philip Rosedale 2013-06-06 16:13:32 -07:00
commit d2eedeb815
1 changed files with 15 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

39
interface/src/SerialInterface.cpp
View file

@ -6,6 +6,7 @@
//
#include "SerialInterface.h"
#include "Util.h"
#include <glm/gtx/vector_angle.hpp>
#include <math.h>
@ -17,7 +18,7 @@
const short NO_READ_MAXIMUM_MSECS = 3000;
const int GRAVITY_SAMPLES = 60; // Use the first few samples to baseline values
const int SENSOR_FUSION_SAMPLES = 200;
const int SENSOR_FUSION_SAMPLES = 20;
const int LONG_TERM_RATE_SAMPLES = 1000;
const bool USING_INVENSENSE_MPU9150 = 1;
@ -214,7 +215,7 @@ void SerialInterface::readData(float deltaTime) {
// From MPU-9150 register map, with setting on
// highest resolution = +/- 2G
_lastAcceleration = glm::vec3(accelXRate, accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
_lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
int rollRate, yawRate, pitchRate;
@ -229,8 +230,8 @@ void SerialInterface::readData(float deltaTime) {
_lastRotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
// Update raw rotation estimates
_estimatedRotation += deltaTime * (_lastRotationRates - _averageRotationRates);
glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
glm::quat(glm::radians(deltaTime * (_lastRotationRates - _averageRotationRates)));
// Update estimated position and velocity
float const DECAY_VELOCITY = 0.95f;
@ -258,29 +259,19 @@ void SerialInterface::readData(float deltaTime) {
1.f/(float)GRAVITY_SAMPLES * _lastAcceleration;
} else {
// Use gravity reading to do sensor fusion on the pitch and roll estimation
float truePitchAngle = glm::angle(glm::normalize(glm::vec3(0, _gravity.y, _gravity.z)),
glm::normalize(glm::vec3(0, _lastAcceleration.y, _lastAcceleration.z)))
* ((_lastAcceleration.z > _gravity.z) ? -1.0 : 1.0);
float trueRollAngle = glm::angle(glm::normalize(glm::vec3(_gravity.x, _gravity.y, 0)),
glm::normalize(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0)))
* ((_lastAcceleration.x > _gravity.x) ? -1.0 : 1.0);
// PER: BUG: This is bizarre, because glm::angle() SOMETIMES returns NaN for what seem to
// be perfectly valid inputs. So I added these NaN tests, gotta fix.
if (!glm::isnan(truePitchAngle) && !glm::isnan(trueRollAngle)) {
_estimatedRotation.x = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.x
+ 1.f/(float)SENSOR_FUSION_SAMPLES * truePitchAngle;
_estimatedRotation.z = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.z
+ 1.f/(float)SENSOR_FUSION_SAMPLES * trueRollAngle;
// Without a compass heading, always decay estimated Yaw slightly
const float YAW_DECAY = 0.995;
_estimatedRotation.y *= YAW_DECAY;
}
estimatedRotation = safeMix(estimatedRotation,
rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation,
1.0f / SENSOR_FUSION_SAMPLES);
// Without a compass heading, always decay estimated Yaw slightly
const float YAW_DECAY = 0.995;
glm::vec3 forward = estimatedRotation * glm::vec3(0.0f, 0.0f, -1.0f);
estimatedRotation = safeMix(glm::angleAxis(glm::degrees(atan2f(forward.x, -forward.z)),
glm::vec3(0.0f, 1.0f, 0.0f)) * estimatedRotation, estimatedRotation, YAW_DECAY);
}
}
_estimatedRotation = safeEulerAngles(estimatedRotation);
totalSamples++;
}