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

Let's see what the long-term average acceleration is.

Browse source
This commit is contained in:
Andrzej Kapolka 2013-06-11 14:23:47 -07:00
parent 47bd3bcdd7
commit 4b890e2ccc
2 changed files with 8 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

7
interface/src/SerialInterface.cpp
View file

@ -218,6 +218,11 @@ void SerialInterface::readData(float deltaTime) {
_lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2; _lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
_averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
1.f/(float)LONG_TERM_RATE_SAMPLES * _lastAcceleration;
printLog("%g %g %g\n", _averageAcceleration.x, _averageAcceleration.y, _averageAcceleration.z);
int rollRate, yawRate, pitchRate; int rollRate, yawRate, pitchRate;
convertHexToInt(sensorBuffer + 22, rollRate); convertHexToInt(sensorBuffer + 22, rollRate);
@ -245,7 +250,7 @@ void SerialInterface::readData(float deltaTime) {
// 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 &&
glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) { (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) {
// compute predicted linear acceleration, find error between actual and predicted // compute predicted linear acceleration, find error between actual and predicted
glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates + glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
_angularAccelToLinearAccel * angularAcceleration; _angularAccelToLinearAccel * angularAcceleration;

2
interface/src/SerialInterface.h
View file

@ -73,6 +73,8 @@ private:
glm::vec3 _lastAcceleration; glm::vec3 _lastAcceleration;
glm::vec3 _lastRotationRates; glm::vec3 _lastRotationRates;
glm::vec3 _averageAcceleration;
glm::mat3 _angularVelocityToLinearAccel; glm::mat3 _angularVelocityToLinearAccel;
glm::mat3 _angularAccelToLinearAccel; glm::mat3 _angularAccelToLinearAccel;
}; };