diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp index a86060775a42b79987643f3c9124c621f97cc825..3268c373015952d02a20468cffcd48144f327bdf 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 c516a488cc44042f1a55114c2de4ddf468a851ad..ca5cc9752b022b613b83fe6fdfcc083ae9044ecf 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);