From 1b8683cbc5371b6603758c32bd9c667eba7e19a4 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Mon, 10 Jun 2013 15:22:53 -0700 Subject: [PATCH] Let's see if we can get an estimate of the distance to the sensor based on the ratios between linear and angular velocity. --- interface/src/SerialInterface.cpp | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index aca91bb34b..26c7f5f581 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -225,9 +225,14 @@ void SerialInterface::readData(float deltaTime) { // Convert the integer rates to floats const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec. - _lastRotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND; - _lastRotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND; - _lastRotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND; + glm::vec3 rotationRates; + rotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND; + rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND; + rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND; + + // compute the angular acceleration + glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime; + _lastRotationRates = rotationRates; // Update raw rotation estimates glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) * @@ -236,6 +241,16 @@ void SerialInterface::readData(float deltaTime) { // Update acceleration estimate _estimatedAcceleration = _lastAcceleration - glm::inverse(estimatedRotation) * _gravity; + static float ratioEstimate = 0.0f; + float angularAccelerationLength = glm::length(angularAcceleration); + float linearAccelerationLength = glm::length(estimatedAcceleration); + if (angularAccelerationLength > EPSILON && linearAccelerationLength > EPSILON) { + float ratio = linearAccelerationLength / angularAccelerationLength; + static float ratioEstimate = ratio; + ratioEstimate = ratioEstimate * 0.999 + ratio * 0.001; + printLog("%g %g\n", ratio, ratioEstimate); + } + // Update estimated position and velocity float const DECAY_VELOCITY = 0.95f; float const DECAY_POSITION = 0.95f;