History

Fixes per code review.

Philip Rosedale committed 13 years ago

Commit 61d4ac21 parent 53e7ef42 files · +14 −14Browse files

Showing the changes to interface/src/SerialInterface.cpp (1 of 2 files). Show all

Unified · Split

interface/src/SerialInterface.cpp+8−8
@@ -259,20 +259,20 @@ void SerialInterface::readData(float deltaTime) { · 258 unmodified lines
259259 } else {
260260 // Use gravity reading to do sensor fusion on the pitch and roll estimation
261261 float truePitchAngle = glm::angle(glm::normalize(glm::vec3(0, _gravity.y, _gravity.z)),
262− glm::normalize(glm::vec3(0, _lastAcceleration.y, _lastAcceleration.z))) *
263− ((_lastAcceleration.z > _gravity.z) ? -1.0 : 1.0);
262+ glm::normalize(glm::vec3(0, _lastAcceleration.y, _lastAcceleration.z)))
263+ * ((_lastAcceleration.z > _gravity.z) ? -1.0 : 1.0);
264264
265265 float trueRollAngle = glm::angle(glm::normalize(glm::vec3(_gravity.x, _gravity.y, 0)),
266− glm::normalize(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0))) *
267− ((_lastAcceleration.x > _gravity.x) ? -1.0 : 1.0);
266+ glm::normalize(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0)))
267+ * ((_lastAcceleration.x > _gravity.x) ? -1.0 : 1.0);
268268
269269 // PER: BUG: This is bizarre, because glm::angle() SOMETIMES returns NaN for what seem to
270270 // be perfectly valid inputs. So I added these NaN tests, gotta fix.
271271 if (!glm::isnan(truePitchAngle) && !glm::isnan(trueRollAngle)) {
272− _estimatedRotation.x = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.x +
273− 1.f/(float)SENSOR_FUSION_SAMPLES * truePitchAngle;
274− _estimatedRotation.z = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.z +
275− 1.f/(float)SENSOR_FUSION_SAMPLES * trueRollAngle;
272+ _estimatedRotation.x = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.x
273+ + 1.f/(float)SENSOR_FUSION_SAMPLES * truePitchAngle;
274+ _estimatedRotation.z = (1.f - 1.f/(float)SENSOR_FUSION_SAMPLES) * _estimatedRotation.z
275+ + 1.f/(float)SENSOR_FUSION_SAMPLES * trueRollAngle;
276276 // Without a compass heading, always decay estimated Yaw slightly
277277 const float YAW_DECAY = 0.995;
278278 _estimatedRotation.y *= YAW_DECAY;