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

Fixes to align better with previous code.

Browse source
This commit is contained in:
Andrzej Kapolka 2013-06-11 15:58:50 -07:00
parent e77aee3967
commit 090df81afe
2 changed files with 13 additions and 10 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

16
interface/src/SerialInterface.cpp
View file

@ -232,15 +232,15 @@ void SerialInterface::readData(float deltaTime) {
rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND; rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND; rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
// update and subtract the long term average
_averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates;
rotationRates -= _averageRotationRates;
// compute the angular acceleration // compute the angular acceleration
glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime; glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
_lastRotationRates = rotationRates; _lastRotationRates = rotationRates;
_averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
1.f/(float)LONG_TERM_RATE_SAMPLES * _lastRotationRates;
printLog("r: %g %g %g\n", _averageRotationRates.x, _averageRotationRates.y, _averageRotationRates.z);
// Update raw rotation estimates // Update raw rotation estimates
glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) * glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
glm::quat(glm::radians(deltaTime * _lastRotationRates)); glm::quat(glm::radians(deltaTime * _lastRotationRates));
@ -249,10 +249,10 @@ void SerialInterface::readData(float deltaTime) {
_estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() : _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
_lastAcceleration - glm::inverse(estimatedRotation) * _gravity; _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
// update and subtract the long term average
_averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration + _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration; 1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
_estimatedAcceleration -= _averageAcceleration;
printLog("a: %g %g %g\n", _averageAcceleration.x, _averageAcceleration.y, _averageAcceleration.z);
// Consider updating our angular velocity/acceleration to linear acceleration mapping // Consider updating our angular velocity/acceleration to linear acceleration mapping
if (glm::length(_estimatedAcceleration) > EPSILON && if (glm::length(_estimatedAcceleration) > EPSILON &&
@ -347,6 +347,8 @@ void SerialInterface::readData(float deltaTime) {
void SerialInterface::resetAverages() { void SerialInterface::resetAverages() {
totalSamples = 0; totalSamples = 0;
_gravity = glm::vec3(0, 0, 0); _gravity = glm::vec3(0, 0, 0);
_averageRotationRates = glm::vec3(0, 0, 0);
_averageAcceleration = glm::vec3(0, 0, 0);
_lastRotationRates = glm::vec3(0, 0, 0); _lastRotationRates = glm::vec3(0, 0, 0);
_estimatedRotation = glm::vec3(0, 0, 0); _estimatedRotation = glm::vec3(0, 0, 0);
_estimatedPosition = glm::vec3(0, 0, 0); _estimatedPosition = glm::vec3(0, 0, 0);

7
interface/src/SerialInterface.h
View file

@ -26,6 +26,8 @@ class SerialInterface {
public: public:
SerialInterface() : active(false), SerialInterface() : active(false),
_gravity(0, 0, 0), _gravity(0, 0, 0),
_averageRotationRates(0, 0, 0),
_averageAcceleration(0, 0, 0),
_estimatedRotation(0, 0, 0), _estimatedRotation(0, 0, 0),
_estimatedPosition(0, 0, 0), _estimatedPosition(0, 0, 0),
_estimatedVelocity(0, 0, 0), _estimatedVelocity(0, 0, 0),
@ -66,6 +68,8 @@ private:
int totalSamples; int totalSamples;
timeval lastGoodRead; timeval lastGoodRead;
glm::vec3 _gravity; glm::vec3 _gravity;
glm::vec3 _averageRotationRates;
glm::vec3 _averageAcceleration;
glm::vec3 _estimatedRotation; glm::vec3 _estimatedRotation;
glm::vec3 _estimatedPosition; glm::vec3 _estimatedPosition;
glm::vec3 _estimatedVelocity; glm::vec3 _estimatedVelocity;
@ -73,9 +77,6 @@ private:
glm::vec3 _lastAcceleration; glm::vec3 _lastAcceleration;
glm::vec3 _lastRotationRates; glm::vec3 _lastRotationRates;
glm::vec3 _averageAcceleration;
glm::vec3 _averageRotationRates;
glm::mat3 _angularVelocityToLinearAccel; glm::mat3 _angularVelocityToLinearAccel;
glm::mat3 _angularAccelToLinearAccel; glm::mat3 _angularAccelToLinearAccel;
}; };