Showing the changes to interface/src/SerialInterface.cpp (1 of 3 files). Show all
Unified · Split
| @@ -236,6 +236,11 @@ void SerialInterface::readData(float deltaTime) { · 235 unmodified lines | ||
| 236 | 236 | glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime; |
| 237 | 237 | _lastRotationRates = rotationRates; |
| 238 | 238 | |
| 239 | + _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates + | |
| 240 | + 1.f/(float)LONG_TERM_RATE_SAMPLES * _lastRotationRates; | |
| 241 | + | |
| 242 | + printLog("r: %g %g %g\n", _averageRotationRates.x, _averageRotationRates.y, _averageRotationRates.z); | |
| 243 | + | |
| 239 | 244 | // Update raw rotation estimates |
| 240 | 245 | glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) * |
| 241 | 246 | glm::quat(glm::radians(deltaTime * _lastRotationRates)); |
| @@ -247,7 +252,7 @@ void SerialInterface::readData(float deltaTime) { · 5 unmodified lines | ||
| 247 | 252 | _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration + |
| 248 | 253 | 1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration; |
| 249 | 254 | |
| 250 | − printLog("%g %g %g\n", _averageAcceleration.x, _averageAcceleration.y, _averageAcceleration.z); | |
| 255 | + printLog("a: %g %g %g\n", _averageAcceleration.x, _averageAcceleration.y, _averageAcceleration.z); | |
| 251 | 256 | |
| 252 | 257 | // Consider updating our angular velocity/acceleration to linear acceleration mapping |
| 253 | 258 | if (glm::length(_estimatedAcceleration) > EPSILON && |