Showing the changes to interface/src/SerialInterface.cpp (1 of 1 file). Show all
Unified · Split
| @@ -262,6 +262,12 @@ void SerialInterface::readData(float deltaTime) { · 261 unmodified lines | |||
| 262 | estimatedRotation = safeMix(estimatedRotation, | 262 | estimatedRotation = safeMix(estimatedRotation, |
| 263 | rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation, | 263 | rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation, |
| 264 | 1.0f / SENSOR_FUSION_SAMPLES); | 264 | 1.0f / SENSOR_FUSION_SAMPLES); |
| 265 | |||
| 266 | // Without a compass heading, always decay estimated Yaw slightly | ||
| 267 | const float YAW_DECAY = 0.995; | ||
| 268 | glm::vec3 forward = estimatedRotation * glm::vec3(0.0f, 0.0f, -1.0f); | ||
| 269 | safeMix(glm::angleAxis(glm::degrees(atan2f(forward.x, -forward.z)), glm::vec3(0.0f, 1.0f, 0.0f)) * | ||
| 270 | estimatedRotation, estimatedRotation, YAW_DECAY); | ||
| 265 | } | 271 | } |
| 266 | } | 272 | } |
| 267 | 273 | ||