Updated the example.

src/examples/bno055_imu_driver_test.cpp

@@ -14,40 +14,35 @@ int main(int argc, char *argv[])
     std::vector<double> accel,mag,gyro;
     std::vector<double> quat,euler,linear_acc,gravity;
     unsigned int i;
-    unsigned char range,bandwidth,mode,rate,op_mode,power_mode;
 
     imu.open("/dev/ttyUSB2");
     try{
       imu.load_calibration("bno055.cal");
+      imu.set_operation_mode(ndof_mode);
     }catch(CException &e){
       std::cout << e.what() << std::endl;
+      imu.set_operation_mode(acc_mag_gyro_mode);
+    }
+
+    if(imu.get_operation_mode()==ndof_mode)
+    {
+      do{
+        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;
+        usleep(100000);
+      }while(!accel_cal || ! mag_cal || !gyro_cal);
     }
-//    do{
-//      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;
-//    }while(!accel_cal || ! mag_cal || !gyro_cal);
     events.push_back(imu.get_new_data_event_id()); 
-    imu.get_accel_config(&range,&bandwidth,&mode);
-    bandwidth=ACCEL_125_BW;
-    imu.set_accel_config(range,bandwidth,mode);
-    imu.get_mag_config(&rate,&op_mode,&power_mode);
-    rate=MAG_30_RATE;
-    imu.set_mag_config(rate,op_mode,power_mode);
-    imu.get_gyro_config(&range,&bandwidth,&mode);
-    bandwidth=GYRO_116_BW;
-    imu.set_gyro_config(range,bandwidth,mode);
-    imu.set_operation_mode(acc_mag_gyro_mode);
     imu.set_data_rate(100);
 
     for(i=0;i<1000;i++)
     {
       auto t1 = std::chrono::high_resolution_clock::now();
       event_server->wait_all(events,1000);
-/*
       quat=imu.get_orientation_quat();      
       std::cout << "Quaternion: X: " << quat[0] << ", Y: " << quat[1] << ", Z: " << quat[2] << ", W: " << quat[3] << std::endl;
       euler=imu.get_orientation_euler();      
@@ -62,7 +57,6 @@ int main(int argc, char *argv[])
       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;
-*/
       auto t2 = std::chrono::high_resolution_clock::now();
       auto duration = std::chrono::duration_cast<std::chrono::microseconds>( t2 - t1 ).count();
       std::cout << duration << std::endl;