From 7aecfa299aa978c127b303cf51874d21adf3342d Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 4 Feb 2020 19:54:42 +0100
Subject: [PATCH] Solved a bug in the function to check if the calibration of
the IMU is done: it returned the inverse value. Completed the IMU example.
---
src/darwin_robot.cpp | 5 +++--
src/examples/darwin_imu_test.cpp | 8 ++++++++
2 files changed, 11 insertions(+), 2 deletions(-)
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index a860607..3268c37 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -353,10 +353,11 @@ bool CDarwinRobot::imu_is_gyro_cal_done(void)
if(this->robot_device==NULL)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
this->robot_device->read_byte_register(IMU_CONTROL_OFFSET,&status);
+ std::cout << std::hex << (int)status << std::endl;
if(status&IMU_CALIBRATING)
- return true;
- else
return false;
+ else
+ return true;
}
bool CDarwinRobot::imu_is_accel_detected(void)
diff --git a/src/examples/darwin_imu_test.cpp b/src/examples/darwin_imu_test.cpp
index c516a48..ca5cc97 100644
--- a/src/examples/darwin_imu_test.cpp
+++ b/src/examples/darwin_imu_test.cpp
@@ -18,6 +18,14 @@ int main(int argc, char *argv[])
std::cout << "found darwin controller" << std::endl;
std::cout << "Number of calibration samples: " << darwin.imu_get_cal_samples() << std::endl;
darwin.imu_start();
+ std::cout << "Gyroscope detected: " << darwin.imu_is_gyro_detected() << std::endl;
+ std::cout << "Accelerometer detected: " << darwin.imu_is_accel_detected() << std::endl;
+ darwin.imu_start_gyro_cal();
+ while(!darwin.imu_is_gyro_cal_done())
+ {
+ std::cout << "calibrating gyro ..." << std::endl;
+ usleep(100000);
+ }
for(i=0;i<50;i++)
{
darwin.imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
--
GitLab