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 | _estimatedVelocity += deltaTime * _estimatedAcceleration; | 301 | _estimatedVelocity += deltaTime * _estimatedAcceleration; |
| 302 | _estimatedPosition += deltaTime * _estimatedVelocity; | 302 | _estimatedPosition += deltaTime * _estimatedVelocity; |
| 303 | _estimatedVelocity *= DECAY_VELOCITY; | 303 | _estimatedVelocity *= DECAY_VELOCITY; |
| 304 | _estimatedPosition *= DECAY_POSITION; | 304 | |
| 305 | 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 | // Accumulate a set of initial baseline readings for setting gravity | 315 | // Accumulate a set of initial baseline readings for setting gravity |
| 307 | if (totalSamples == 0) { | 316 | if (totalSamples == 0) { |
| 308 | _gravity = _lastAcceleration; | 317 | _gravity = _lastAcceleration; |
| @@ -327,6 +336,12 @@ void SerialInterface::readData(float deltaTime) { · 18 unmodified lines | |||
| 327 | 336 | ||
| 328 | _estimatedRotation = safeEulerAngles(estimatedRotation); | 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 | totalSamples++; | 345 | totalSamples++; |
| 331 | } | 346 | } |
| 332 | 347 | ||