Showing the changes to interface/src/SerialInterface.cpp (1 of 1 file). Show all
Unified · Split
| @@ -240,10 +240,12 @@ void SerialInterface::readData(float deltaTime) { · 239 unmodified lines | ||
| 240 | 240 | glm::quat(glm::radians(deltaTime * _lastRotationRates)); |
| 241 | 241 | |
| 242 | 242 | // Update acceleration estimate: first, subtract gravity as rotated into current frame |
| 243 | − _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity; | |
| 243 | + _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() : | |
| 244 | + _lastAcceleration - glm::inverse(estimatedRotation) * _gravity; | |
| 244 | 245 | |
| 245 | 246 | // Consider updating our angular velocity/acceleration to linear acceleration mapping |
| 246 | − if (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) { | |
| 247 | + if (glm::length(_estimatedAcceleration) > EPSILON && | |
| 248 | + glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON) { | |
| 247 | 249 | // compute predicted linear acceleration, find error between actual and predicted |
| 248 | 250 | glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates + |
| 249 | 251 | _angularAccelToLinearAccel * angularAcceleration; |
| @@ -338,6 +340,7 @@ void SerialInterface::resetAverages() { · 88 unmodified lines | ||
| 338 | 340 | _estimatedRotation = glm::vec3(0, 0, 0); |
| 339 | 341 | _estimatedPosition = glm::vec3(0, 0, 0); |
| 340 | 342 | _estimatedVelocity = glm::vec3(0, 0, 0); |
| 343 | + _estimatedAcceleration = glm::vec3(0, 0, 0); | |
| 341 | 344 | } |
| 342 | 345 | |
| 343 | 346 | void SerialInterface::resetSerial() { |