Don't estimate acceleration before we know the gravity and don't update our matrices without acceleration.

Andrzej Kapolka committed 13 years ago

Commit 47bd3bc1 parent b88380d1 file · +5 −2Browse files

Showing the changes to interface/src/SerialInterface.cpp (1 of 1 file). Show all

Unified · Split

             glm::quat(glm::radians(deltaTime * _lastRotationRates));
         //  Update acceleration estimate: first, subtract gravity as rotated into current frame
-        _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
+        _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
+            _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
         //  Consider updating our angular velocity/acceleration to linear acceleration mapping
-        if (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) {
+        if (glm::length(_estimatedAcceleration) > EPSILON &&
+                glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) {
             // compute predicted linear acceleration, find error between actual and predicted
             glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
                 _angularAccelToLinearAccel * angularAcceleration;
     _estimatedRotation = glm::vec3(0, 0, 0);
     _estimatedPosition = glm::vec3(0, 0, 0);
     _estimatedVelocity = glm::vec3(0, 0, 0);
+    _estimatedAcceleration = glm::vec3(0, 0, 0);
 }
 void SerialInterface::resetSerial() {