Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-23 15:13:41 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Remove average measurements, compute estimated acceleration (without rotated

Browse source 
gravity).
This commit is contained in:
Andrzej Kapolka 2013-06-10 14:49:39 -07:00
parent fb97d3e04b
commit 5820c3c7c3
2 changed files with 13 additions and 22 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

23
interface/src/SerialInterface.cpp
View file

@ -155,11 +155,11 @@ void SerialInterface::renderLevels(int width, int height) {
// Acceleration rates
glColor4f(1, 1, 1, 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 42);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.x - _gravity.x) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 42);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.x * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 42);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 57);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.y - _gravity.y) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 57);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.y * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 57);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 72);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.z - _gravity.z) * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.z * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72);
// Estimated Position
glColor4f(0, 1, 1, 1);
@ -231,29 +231,24 @@ void SerialInterface::readData(float deltaTime) {
// Update raw rotation estimates
glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
glm::quat(glm::radians(deltaTime * (_lastRotationRates - _averageRotationRates)));
glm::quat(glm::radians(deltaTime * _lastRotationRates));
// Update acceleration estimate
_estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
// Update estimated position and velocity
float const DECAY_VELOCITY = 0.95f;
float const DECAY_POSITION = 0.95f;
_estimatedVelocity += deltaTime * (_lastAcceleration - _averageAcceleration);
_estimatedVelocity += deltaTime * _estimatedAcceleration;
_estimatedPosition += deltaTime * _estimatedVelocity;
_estimatedVelocity *= DECAY_VELOCITY;
_estimatedPosition *= DECAY_POSITION;
// Accumulate a set of initial baseline readings for setting gravity
if (totalSamples == 0) {
_averageRotationRates = _lastRotationRates;
_averageAcceleration = _lastAcceleration;
_gravity = _lastAcceleration;
}
else {
// Cumulate long term average to (hopefully) take DC bias out of rotation rates
_averageRotationRates = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates
+ 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastRotationRates;
_averageAcceleration = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration
+ 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastAcceleration;
if (totalSamples < GRAVITY_SAMPLES) {
_gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity +
1.f/(float)GRAVITY_SAMPLES * _lastAcceleration;
@ -293,8 +288,6 @@ void SerialInterface::readData(float deltaTime) {
void SerialInterface::resetAverages() {
totalSamples = 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);
_estimatedRotation = glm::vec3(0, 0, 0);
_estimatedPosition = glm::vec3(0, 0, 0);

12
interface/src/SerialInterface.h
View file

@ -26,8 +26,6 @@ class SerialInterface {
public:
SerialInterface() : active(false),
_gravity(0, 0, 0),
_averageRotationRates(0, 0, 0),
_averageAcceleration(0, 0, 0),
_estimatedRotation(0, 0, 0),
_estimatedPosition(0, 0, 0),
_estimatedVelocity(0, 0, 0),
@ -37,13 +35,14 @@ public:
void pair();
void readData(float deltaTime);
const float getLastPitchRate() const { return _lastRotationRates[0] - _averageRotationRates[0]; }
const float getLastYawRate() const { return _lastRotationRates[1] - _averageRotationRates[1]; }
const float getLastRollRate() const { return _lastRotationRates[2] - _averageRotationRates[2]; }
const float getLastPitchRate() const { return _lastRotationRates[0]; }
const float getLastYawRate() const { return _lastRotationRates[1]; }
const float getLastRollRate() const { return _lastRotationRates[2]; }
const glm::vec3& getLastRotationRates() const { return _lastRotationRates; };
const glm::vec3& getEstimatedRotation() const { return _estimatedRotation; };
const glm::vec3& getEstimatedPosition() const { return _estimatedPosition; };
const glm::vec3& getEstimatedVelocity() const { return _estimatedVelocity; };
const glm::vec3& getEstimatedAcceleration() const { return _estimatedAcceleration; };
const glm::vec3& getLastAcceleration() const { return _lastAcceleration; };
const glm::vec3& getGravity() const { return _gravity; };
@ -59,11 +58,10 @@ private:
int totalSamples;
timeval lastGoodRead;
glm::vec3 _gravity;
glm::vec3 _averageRotationRates;
glm::vec3 _averageAcceleration;
glm::vec3 _estimatedRotation;
glm::vec3 _estimatedPosition;
glm::vec3 _estimatedVelocity;
glm::vec3 _estimatedAcceleration;
glm::vec3 _lastAcceleration;
glm::vec3 _lastRotationRates;
};