From de2e5d0dd895977d794a21337771c62ff5ec7b79 Mon Sep 17 00:00:00 2001
From: Timothee Habra <timothee.habra@uclouvain.be>
Date: Wed, 21 Dec 2016 13:29:24 +0100
Subject: [PATCH] add an option to estimate and remove IMU gyroscope ofset on
initialization
---
source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp | 39 +++++++++++++++++--
source/RobotAPI/drivers/XsensIMU/XsensIMU.h | 1 +
2 files changed, 36 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp b/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
index 03536408a..77659db22 100644
--- a/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
+++ b/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
@@ -25,7 +25,6 @@
#include "XsensIMU.h"
-
using namespace armarx;
using namespace IMU;
@@ -37,6 +36,38 @@ PropertyDefinitionsPtr XsensIMU::createPropertyDefinitions()
void XsensIMU::frameAcquisitionTaskLoop()
{
+
+ std::vector<float> offset(3, 0.0);
+
+ int count = 0;
+
+ IceUtil::Time startTime = armarx::TimeUtil::GetTime();
+
+ if (getProperty<bool>("EnableSimpleCalibration").getValue())
+ {
+ ARMARX_WARNING << "Estimation of the offset for the IMU, please do not move the IMU";
+ while (((armarx::TimeUtil::GetTime() - startTime) < IceUtil::Time::seconds(5)) && !sensorTask->isStopped())
+ {
+
+ while (HasPendingEvents())
+ {
+ ProcessPendingEvent();
+
+ offset[0] += m_GyroscopeRotation[0];
+ offset[1] += m_GyroscopeRotation[1];
+ offset[2] += m_GyroscopeRotation[2];
+
+ count ++;
+ }
+ }
+
+ offset[0] /= count;
+ offset[1] /= count;
+ offset[2] /= count;
+
+ }
+
+
while (!sensorTask->isStopped())
{
@@ -52,9 +83,9 @@ void XsensIMU::frameAcquisitionTaskLoop()
data.acceleration.push_back(m_Accelaration[1]);
data.acceleration.push_back(m_Accelaration[2]);
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[0]);
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[1]);
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[2]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[0] - offset[0]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[1] - offset[1]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[2] - offset[2]);
data.magneticRotation.push_back(m_MagneticRotation[0]);
diff --git a/source/RobotAPI/drivers/XsensIMU/XsensIMU.h b/source/RobotAPI/drivers/XsensIMU/XsensIMU.h
index 7d91988e5..1243182f6 100644
--- a/source/RobotAPI/drivers/XsensIMU/XsensIMU.h
+++ b/source/RobotAPI/drivers/XsensIMU/XsensIMU.h
@@ -52,6 +52,7 @@ namespace armarx
defineOptionalProperty<std::string>("frequency", "", "");
defineOptionalProperty<std::string>("maxPendingEvents", "", "");
+ defineOptionalProperty<bool>("EnableSimpleCalibration", false, "");
}
};
--
GitLab