Showing the changes to interface/src/SerialInterface.cpp (1 of 1 file). Show all
Unified · Split
| @@ -301,8 +301,17 @@ void SerialInterface::readData(float deltaTime) { · 300 unmodified lines | ||
| 301 | 301 | _estimatedVelocity += deltaTime * _estimatedAcceleration; |
| 302 | 302 | _estimatedPosition += deltaTime * _estimatedVelocity; |
| 303 | 303 | _estimatedVelocity *= DECAY_VELOCITY; |
| 304 | − _estimatedPosition *= DECAY_POSITION; | |
| 305 | − | |
| 304 | + | |
| 305 | + // Attempt to fuse gyro position with webcam position | |
| 306 | + Webcam* webcam = Application::getInstance()->getWebcam(); | |
| 307 | + if (webcam->isActive()) { | |
| 308 | + _estimatedPosition = glm::mix(_estimatedPosition, webcam->getEstimatedPosition(), | |
| 309 | + 1.0f / SENSOR_FUSION_SAMPLES); | |
| 310 | + | |
| 311 | + } else { | |
| 312 | + _estimatedPosition *= DECAY_POSITION; | |
| 313 | + } | |
| 314 | + | |
| 306 | 315 | // Accumulate a set of initial baseline readings for setting gravity |
| 307 | 316 | if (totalSamples == 0) { |
| 308 | 317 | _gravity = _lastAcceleration; |
| @@ -327,6 +336,12 @@ void SerialInterface::readData(float deltaTime) { · 18 unmodified lines | ||
| 327 | 336 | |
| 328 | 337 | _estimatedRotation = safeEulerAngles(estimatedRotation); |
| 329 | 338 | |
| 339 | + // Fuse gyro roll with webcam roll | |
| 340 | + if (webcam->isActive()) { | |
| 341 | + _estimatedRotation.z = glm::mix(_estimatedRotation.z, webcam->getEstimatedRotation().z, | |
| 342 | + 1.0f / SENSOR_FUSION_SAMPLES); | |
| 343 | + } | |
| 344 | + | |
| 330 | 345 | totalSamples++; |
| 331 | 346 | } |
| 332 | 347 | |