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 | 262 | estimatedRotation = safeMix(estimatedRotation, |
| 263 | 263 | rotationBetween(estimatedRotation * _lastAcceleration, _gravity) * estimatedRotation, |
| 264 | 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 | |