diff --git a/src/examples/bno055_imu_driver_test.cpp b/src/examples/bno055_imu_driver_test.cpp
index d5722fe25b428aa81feaefa8f232943e817432f0..c003e48119492c8168943da635759a44b6960982 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){