Showing the changes to interface/src/SerialInterface.cpp (1 of 2 files). Show all
Unified · Split
| @@ -259,20 +259,20 @@ void SerialInterface::readData(float deltaTime) { · 258 unmodified lines | |||
| 259 | } else { | 259 | } else { |
| 260 | // Use gravity reading to do sensor fusion on the pitch and roll estimation | 260 | // Use gravity reading to do sensor fusion on the pitch and roll estimation |
| 261 | float truePitchAngle = glm::angle(glm::normalize(glm::vec3(0, _gravity.y, _gravity.z)), | 261 | float truePitchAngle = glm::angle(glm::normalize(glm::vec3(0, _gravity.y, _gravity.z)), |
| 262 | glm::normalize(glm::vec3(0, _lastAcceleration.y, _lastAcceleration.z))) * | 262 | glm::normalize(glm::vec3(0, _lastAcceleration.y, _lastAcceleration.z))) |
| 263 | ((_lastAcceleration.z > _gravity.z) ? -1.0 : 1.0); | 263 | * ((_lastAcceleration.z > _gravity.z) ? -1.0 : 1.0); |
| 264 | 264 | ||
| 265 | float trueRollAngle = glm::angle(glm::normalize(glm::vec3(_gravity.x, _gravity.y, 0)), | 265 | float trueRollAngle = glm::angle(glm::normalize(glm::vec3(_gravity.x, _gravity.y, 0)), |
| 266 | glm::normalize(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0))) * | 266 | glm::normalize(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0))) |
| 267 | ((_lastAcceleration.x > _gravity.x) ? -1.0 : 1.0); | 267 | * ((_lastAcceleration.x > _gravity.x) ? -1.0 : 1.0); |
| 268 | 268 | ||
| 269 | // PER: BUG: This is bizarre, because glm::angle() SOMETIMES returns NaN for what seem to | 269 | // PER: BUG: This is bizarre, because glm::angle() SOMETIMES returns NaN for what seem to |
| 270 | // be perfectly valid inputs. So I added these NaN tests, gotta fix. | 270 | // be perfectly valid inputs. So I added these NaN tests, gotta fix. |
| 271 | if (!glm::isnan(truePitchAngle) && !glm::isnan(trueRollAngle)) { | 271 | if (!glm::isnan(truePitchAngle) && !glm::isnan(trueRollAngle)) { |
| 272 | _estimatedRotation.x = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.x + | 272 | _estimatedRotation.x = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.x |
| 273 | 1.f/(float)SENSOR_FUSION_SAMPLES * truePitchAngle; | 273 | + 1.f/(float)SENSOR_FUSION_SAMPLES * truePitchAngle; |
| 274 | _estimatedRotation.z = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.z + | 274 | _estimatedRotation.z = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.z |
| 275 | 1.f/(float)SENSOR_FUSION_SAMPLES * trueRollAngle; | 275 | + 1.f/(float)SENSOR_FUSION_SAMPLES * trueRollAngle; |
| 276 | // Without a compass heading, always decay estimated Yaw slightly | 276 | // Without a compass heading, always decay estimated Yaw slightly |
| 277 | const float YAW_DECAY = 0.995; | 277 | const float YAW_DECAY = 0.995; |
| 278 | _estimatedRotation.y *= YAW_DECAY; | 278 | _estimatedRotation.y *= YAW_DECAY; |