Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-23 15:13:41 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Let's see if we can get an estimate of the distance to the sensor based on

Browse source 
the ratios between linear and angular velocity.
This commit is contained in:
Andrzej Kapolka 2013-06-10 15:22:53 -07:00
parent 5820c3c7c3
commit 1b8683cbc5
1 changed files with 18 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

21
interface/src/SerialInterface.cpp
View file

@ -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;