From a3f83cbdab53129747a8049b2443dcf8dbba50a8 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Mon, 3 Feb 2020 15:25:55 +0100
Subject: [PATCH] Modified the example to continue even if the IMU was not
calibrated.
---
src/examples/bno055_imu_driver_test.cpp | 35 ++++++++-----------------
1 file changed, 11 insertions(+), 24 deletions(-)
diff --git a/src/examples/bno055_imu_driver_test.cpp b/src/examples/bno055_imu_driver_test.cpp
index d5722fe..c003e48 100644
--- a/src/examples/bno055_imu_driver_test.cpp
+++ b/src/examples/bno055_imu_driver_test.cpp
@@ -14,34 +14,21 @@ int main(int argc, char *argv[])
std::vector<double> quat,euler,linear_acc,gravity;
unsigned int i;
- imu.open("/dev/pts/23");
+ imu.open("/dev/ttyUSB2");
try{
imu.load_calibration("bno055.cal");
- do{
- imu.set_operation_mode(ndof_mode);
- accel_cal=imu.is_accelerometer_calibrated();
- std::cout << "Accelerometer calibrated: " << accel_cal << std::endl;
- mag_cal=imu.is_magnetometer_calibrated();
- std::cout << "Magnetometer calibrated: " << mag_cal << std::endl;
- gyro_cal=imu.is_gyroscope_calibrated();
- std::cout << "Gyroscope calibrated: " << gyro_cal << std::endl;
- sleep(1);
- }while(!accel_cal || !mag_cal || !gyro_cal);
}catch(CException &e){
std::cout << e.what() << std::endl;
- do{
- imu.set_operation_mode(ndof_mode);
- accel_cal=imu.is_accelerometer_calibrated();
- std::cout << "Accelerometer calibrated: " << accel_cal << std::endl;
- mag_cal=imu.is_magnetometer_calibrated();
- std::cout << "Magnetometer calibrated: " << mag_cal << std::endl;
- gyro_cal=imu.is_gyroscope_calibrated();
- std::cout << "Gyroscope calibrated: " << gyro_cal << std::endl;
- sleep(1);
- }while(!accel_cal || !mag_cal || !gyro_cal);
- imu.save_calibration("bno055.cal");
}
+ imu.set_operation_mode(acc_mag_gyro_mode);
+ accel_cal=imu.is_accelerometer_calibrated();
+ std::cout << "Accelerometer calibrated: " << accel_cal << std::endl;
+ mag_cal=imu.is_magnetometer_calibrated();
+ std::cout << "Magnetometer calibrated: " << mag_cal << std::endl;
+ gyro_cal=imu.is_gyroscope_calibrated();
+ std::cout << "Gyroscope calibrated: " << gyro_cal << std::endl;
events.push_back(imu.get_new_data_event_id());
+
for(i=0;i<1000;i++)
{
event_server->wait_all(events,1000);
@@ -53,12 +40,12 @@ int main(int argc, char *argv[])
std::cout << "Linear acceleration: X: " << linear_acc[0] << ", Y: " << linear_acc[1] << ", Z: " << linear_acc[2] << std::endl;
gravity=imu.get_gravity();
std::cout << "Gravity: X: " << gravity[0] << ", Y: " << gravity[1] << ", Z: " << gravity[2] << std::endl;
-/* accel=imu.get_raw_accelerometer();
+ accel=imu.get_raw_accelerometer();
std::cout << "Accelerometer: X: " << accel[0] << ", Y: " << accel[1] << ", Z: " << accel[2] << std::endl;
mag=imu.get_raw_magnetometer();
std::cout << "Magnetometer: X: " << mag[0] << ", Y: " << mag[1] << ", Z: " << mag[2] << std::endl;
gyro=imu.get_raw_gyroscope();
- std::cout << "Gyroscope: X: " << gyro[0] << ", Y: " << gyro[1] << ", Z: " << gyro[2] << std::endl;*/
+ std::cout << "Gyroscope: X: " << gyro[0] << ", Y: " << gyro[1] << ", Z: " << gyro[2] << std::endl;
}
}catch(CException &e){
--
GitLab