From b0fa26e39038bac544dd97468f7e932c0c4f6d0a Mon Sep 17 00:00:00 2001 From: Dante Ruiz Date: Tue, 16 May 2017 16:38:01 +0100 Subject: [PATCH] fix wierd calibration effect due to low velocity filter --- .../src/controllers/impl/filters/LowVelocityFilter.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp b/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp index b47a78c133..594f538369 100644 --- a/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp +++ b/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp @@ -18,7 +18,7 @@ namespace controller { Pose LowVelocityFilter::apply(Pose newPose) const { Pose finalPose = newPose; - if (finalPose.isValid()) { + if (finalPose.isValid() && _oldPose.isValid()) { float rotationFilter = glm::clamp(1.0f - (glm::length(_oldPose.getVelocity() / _rotationConstant)), 0.0f, 1.0f); float translationFilter = glm::clamp(1.0f - (glm::length(_oldPose.getVelocity() / _translationConstant)), 0.0f, 1.0f); finalPose.translation = _oldPose.getTranslation() * translationFilter + newPose.getTranslation() * (1.0f - translationFilter); @@ -35,11 +35,9 @@ namespace controller { if (obj.contains(JSON_ROTATION) && obj.contains(JSON_TRANSLATION)) { _rotationConstant = obj[JSON_ROTATION].toDouble(); _translationConstant = obj[JSON_TRANSLATION].toDouble(); - qDebug() << "--------->Successfully parsed low velocity filter"; return true; } } - qDebug() << "--------->failed parsed low velocity filter"; return false; }