From 7e34bef01edcbeda2476ce189b5c76e64d14a92c Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 20 Jun 2013 16:07:33 -0700 Subject: [PATCH] Attempt to fuse gyro readings with webcam data. --- interface/src/SerialInterface.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 94ea5d58d6..4647028730 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -301,8 +301,17 @@ void SerialInterface::readData(float deltaTime) { _estimatedVelocity += deltaTime * _estimatedAcceleration; _estimatedPosition += deltaTime * _estimatedVelocity; _estimatedVelocity *= DECAY_VELOCITY; - _estimatedPosition *= DECAY_POSITION; - + + // Attempt to fuse gyro position with webcam position + Webcam* webcam = Application::getInstance()->getWebcam(); + if (webcam->isActive()) { + _estimatedPosition = glm::mix(_estimatedPosition, webcam->getEstimatedPosition(), + 1.0f / SENSOR_FUSION_SAMPLES); + + } else { + _estimatedPosition *= DECAY_POSITION; + } + // Accumulate a set of initial baseline readings for setting gravity if (totalSamples == 0) { _gravity = _lastAcceleration; @@ -327,6 +336,12 @@ void SerialInterface::readData(float deltaTime) { _estimatedRotation = safeEulerAngles(estimatedRotation); + // Fuse gyro roll with webcam roll + if (webcam->isActive()) { + _estimatedRotation.z = glm::mix(_estimatedRotation.z, webcam->getEstimatedRotation().z, + 1.0f / SENSOR_FUSION_SAMPLES); + } + totalSamples++; }