diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..40ebf8588f87960db1b3c5ccbab7c04fe7909d24
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+bin
+lib
+build
diff --git a/Findbno055_imu_sim.cmake b/Findbno055_imu_sim.cmake
deleted file mode 100644
index d535c23e11c081b8d798698db82b51dee5f9e9d0..0000000000000000000000000000000000000000
--- a/Findbno055_imu_sim.cmake
+++ /dev/null
@@ -1,21 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(bno055_imu_sim_INCLUDE_DIR bno055_imu_sim.h /usr/include/iridrivers /usr/local/include/iridrivers)
-
-FIND_LIBRARY(bno055_imu_sim_LIBRARY
-    NAMES bno055_imu_sim
-    PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers) 
-
-IF (bno055_imu_sim_INCLUDE_DIR AND bno055_imu_sim_LIBRARY)
-   SET(bno055_imu_sim_FOUND TRUE)
-ENDIF (bno055_imu_sim_INCLUDE_DIR AND bno055_imu_sim_LIBRARY)
-
-IF (bno055_imu_sim_FOUND)
-   IF (NOT bno055_imu_sim_FIND_QUIETLY)
-      MESSAGE(STATUS "Found bno055_imu_sim: ${bno055_imu_sim_LIBRARY}")
-   ENDIF (NOT bno055_imu_sim_FIND_QUIETLY)
-ELSE (bno055_imu_sim_FOUND)
-   IF (bno055_imu_sim_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find bno055_imu_sim")
-   ENDIF (bno055_imu_sim_FIND_REQUIRED)
-ENDIF (bno055_imu_sim_FOUND)
-
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b7b96683553aa06999ff893d498c587e14bd3ad5..727e4a2c09b02aecf1cb774bb08d9940ab3ca22f 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -2,10 +2,6 @@
 SET(sources bno055_imu_driver.cpp bno055_imu_exceptions.cpp)
 # application header files
 SET(headers bno055_imu_driver.h bno055_imu_exceptions.h bno055_common.h)
-# simulator source files
-SET(sources_sim bno055_imu_sim.cpp bno055_imu_exceptions.cpp)
-# application header files
-SET(headers_sim bno055_imu_sim.h bno055_imu_exceptions.h bno055_common.h)
 # locate the necessary dependencies
 FIND_PACKAGE(iriutils REQUIRED)
 FIND_PACKAGE(comm REQUIRED)
@@ -19,23 +15,11 @@ ADD_LIBRARY(bno055_imu_driver SHARED ${sources})
 TARGET_LINK_LIBRARIES(bno055_imu_driver ${iriutils_LIBRARY})
 TARGET_LINK_LIBRARIES(bno055_imu_driver ${comm_LIBRARY})
 
-# create the shared library
-ADD_LIBRARY(bno055_imu_sim SHARED ${sources_sim})
-# link necessary libraries
-TARGET_LINK_LIBRARIES(bno055_imu_sim ${iriutils_LIBRARY})
-TARGET_LINK_LIBRARIES(bno055_imu_sim ${comm_LIBRARY})
-
 INSTALL(TARGETS bno055_imu_driver
         RUNTIME DESTINATION bin
         LIBRARY DESTINATION lib/iridrivers
         ARCHIVE DESTINATION lib/iridrivers)
 INSTALL(FILES ${headers} DESTINATION include/iridrivers)
-INSTALL(TARGETS bno055_imu_sim
-        RUNTIME DESTINATION bin
-        LIBRARY DESTINATION lib/iridrivers
-        ARCHIVE DESTINATION lib/iridrivers)
-INSTALL(FILES ${headers_sim} DESTINATION include/iridrivers)
 INSTALL(FILES ../Findbno055_imu_driver.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
-INSTALL(FILES ../Findbno055_imu_sim.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
 
 ADD_SUBDIRECTORY(examples)
diff --git a/src/bno055_imu_driver.cpp b/src/bno055_imu_driver.cpp
index bcf989973dd2a01b5e7dce36743e2cc2544eee03..1475131140bcb7d48a87096b894f785a3a223ba8 100644
--- a/src/bno055_imu_driver.cpp
+++ b/src/bno055_imu_driver.cpp
@@ -23,6 +23,10 @@ CBNO055IMUDriver::CBNO055IMUDriver(const std::string &name)
     this->info.soft_ver=0xFFFF;
     this->info.boot_ver=0xFF;
     this->current_page=0x00;
+    this->accel_calibrated=false;
+    this->mag_calibrated=false;
+    this->gyro_calibrated=false;
+    this->system_calibrated=false;
     /* initialize events */
     this->event_server=CEventServer::instance();
     this->finish_thread_event_id=this->imu_name+"_finish_thread_event";
@@ -43,6 +47,7 @@ CBNO055IMUDriver::CBNO055IMUDriver(const std::string &name)
     this->euler_angles_data.resize(3,0.0);
     this->linear_accel_data.resize(3,0.0);
     this->gravity_data.resize(3,0.0);
+    this->temperature=0.0;
   }
 }
 
@@ -84,10 +89,20 @@ void CBNO055IMUDriver::convert_gyro_data(unsigned char *data)
 
 void CBNO055IMUDriver::convert_quaternion_data(unsigned char *data)
 {
-  this->quaternion_data[3]=((signed short int)(data[0]+(data[1]<<8)))/16384.0;
-  this->quaternion_data[0]=((signed short int)(data[2]+(data[3]<<8)))/16384.0;
-  this->quaternion_data[1]=((signed short int)(data[4]+(data[5]<<8)))/16384.0;
-  this->quaternion_data[2]=((signed short int)(data[6]+(data[7]<<8)))/16384.0;
+  if(this->op_mode>=imu_mode)
+  {
+    this->quaternion_data[3]=((signed short int)(data[0]+(data[1]<<8)))/16384.0;
+    this->quaternion_data[0]=((signed short int)(data[2]+(data[3]<<8)))/16384.0;
+    this->quaternion_data[1]=((signed short int)(data[4]+(data[5]<<8)))/16384.0;
+    this->quaternion_data[2]=((signed short int)(data[6]+(data[7]<<8)))/16384.0;
+  }
+  else
+  {
+    this->quaternion_data[3]=1.0;
+    this->quaternion_data[0]=0.0;
+    this->quaternion_data[1]=0.0;
+    this->quaternion_data[2]=0.0;
+  }
 }
 
 void CBNO055IMUDriver::convert_euler_angles_data(unsigned char *data)
@@ -132,6 +147,34 @@ void CBNO055IMUDriver::convert_gravity_data(unsigned char *data)
   }
 }
 
+void CBNO055IMUDriver::convert_temperature_data(unsigned char *data)
+{
+  if(this->temp_units==temp_celcius)
+    this->temperature=(double)data[0];
+  else
+    this->temperature=(double)(data[0]*2.0);
+}
+
+void CBNO055IMUDriver::convert_calibration_data(unsigned char *data)
+{
+  if((data[0]&0x03)==0x03)
+    this->mag_calibrated=true;
+  else
+    this->mag_calibrated=false;
+  if((data[0]&0x0C)==0x0C)
+    this->accel_calibrated=true;
+  else
+    this->accel_calibrated=false;
+  if((data[0]&0x30)==0x30)
+    this->gyro_calibrated=true;
+  else
+    this->gyro_calibrated=false;
+  if((data[0]&0xC0)==0xC0)
+    this->system_calibrated=true;
+  else
+    this->system_calibrated=false;
+}
+
 void CBNO055IMUDriver::change_register_page(unsigned char page_id)
 {
   if(page_id!=0x01 && page_id!=0x00)
@@ -149,45 +192,22 @@ void CBNO055IMUDriver::change_register_page(unsigned char page_id)
 void CBNO055IMUDriver::process_data(unsigned char *data)
 {
   this->data_access.enter();
-  switch(this->op_mode)
-  {
-    case config_mode: break;
-    case acc_only_mode: this->convert_accel_data(data);
-			break;
-    case mag_only_mode: this->convert_mag_data(data);
-			break;
-    case gyro_only_mode: this->convert_gyro_data(data);
-			 break;
-    case acc_mag_mode: this->convert_accel_data(data);
-                       this->convert_mag_data(&data[6]);
-		       break;
-    case acc_gyro_mode: this->convert_accel_data(data);
-                        this->convert_gyro_data(&data[6]);
-			break;
-    case mag_gyro_mode: this->convert_mag_data(data);
-                        this->convert_gyro_data(&data[6]);
-			break;
-    case acc_mag_gyro_mode: this->convert_accel_data(data);
-                            this->convert_mag_data(&data[6]);
-                            this->convert_gyro_data(&data[12]);
-			    break;
-    case imu_mode:
-    case compass_mode:
-    case m4g_mode:
-    case ndof_off_mode:
-    case ndof_mode: this->convert_euler_angles_data(data);
-                    this->convert_quaternion_data(&data[6]);
-                    this->convert_linear_accel_data(&data[14]);
-                    this->convert_gravity_data(&data[20]);
-		    break;
-  }
+  this->convert_accel_data(data);
+  this->convert_mag_data(&data[6]);
+  this->convert_gyro_data(&data[12]);
+  this->convert_euler_angles_data(&data[18]);
+  this->convert_quaternion_data(&data[24]);
+  this->convert_linear_accel_data(&data[32]);
+  this->convert_gravity_data(&data[38]);
+  this->convert_temperature_data(&data[44]);
+  this->convert_calibration_data(&data[45]);
   this->data_access.exit();
 }
 
 void *CBNO055IMUDriver::data_thread(void *param)
 {
   CBNO055IMUDriver *imu=(CBNO055IMUDriver *)param;
-  unsigned char address,length,*data;
+  unsigned char address=0x08,length=46,data[46];
   bool end=false;
 
   while(!end)
@@ -196,49 +216,7 @@ void *CBNO055IMUDriver::data_thread(void *param)
       end=true;
     else
     {
-      switch(imu->op_mode)
-      {
-        case config_mode: address=0x00;
-                          length=0x00;
-                          break;
-        case acc_only_mode: address=0x08;
-                            length=6;
-                            data=new unsigned char[length];
-                            break;
-        case mag_only_mode: address=0x0E;
-                            length=6;
-                            data=new unsigned char[length];
-                            break;
-        case gyro_only_mode: address=0x14;
-                             length=6;
-                             data=new unsigned char[length];
-                             break;
-        case acc_mag_mode: address=0x08;
-                           length=12;
-                           data=new unsigned char[length];
-                           break;
-        case acc_gyro_mode: address=0x08;
-                            length=18;
-                            data=new unsigned char[length];
-                            break;
-        case mag_gyro_mode: address=0x0E;
-                            length=12;
-                            data=new unsigned char[length];
-                            break;
-        case acc_mag_gyro_mode: address=0x08;
-                                length=18;
-                                data=new unsigned char[length];
-                                break;
-        case imu_mode: 
-        case compass_mode: 
-        case m4g_mode: 
-        case ndof_off_mode: 
-        case ndof_mode: address=0x1A;
-                        length=26;
-                        data=new unsigned char[length];
-                        break;
-      }
-      if(length>0)
+      if(imu->op_mode!=config_mode)
       {
         try{
           imu->imu_access.enter();
@@ -246,11 +224,8 @@ void *CBNO055IMUDriver::data_thread(void *param)
           imu->read_registers(address,length,data);
           imu->imu_access.exit();
           imu->process_data(data);
-          delete[] data;
           if(!imu->event_server->event_is_set(imu->new_data_event_id))
-          {
             imu->event_server->set_event(imu->new_data_event_id);
-          }
         }catch(CException &e){
           imu->imu_access.exit();
           std::cout << e.what() << std::endl;
@@ -464,6 +439,7 @@ void CBNO055IMUDriver::get_sensor_configs(void)
 void CBNO055IMUDriver::open(const std::string &serial_dev)
 {
   TRS232_config config;
+  unsigned char cal_data;
 
   try{
     this->imu_port=new CRS232(this->imu_name+"_serial_port");
@@ -477,6 +453,8 @@ void CBNO055IMUDriver::open(const std::string &serial_dev)
     this->detect_sensor();
     /* get the current state of the device */
     this->read_registers(0x07,1,&this->current_page);
+    this->read_registers(0x35,1,&cal_data);
+    this->convert_calibration_data(&cal_data);
     this->op_mode=this->get_operation_mode();
     this->set_operation_mode(config_mode);
     this->power_mode=this->get_power_mode();
@@ -533,83 +511,38 @@ void CBNO055IMUDriver::set_operation_mode(op_mode_t op_mode)
 {
   if(this->op_mode!=op_mode)
   {
-    try{
-      this->imu_access.enter();
-      this->change_register_page(0x00);
-      this->write_registers(0x3D,1,(unsigned char *)&op_mode);
-      this->imu_access.exit();
-    }catch(CException &e){
-      this->imu_access.exit();
-      throw;
-    }
-    this->op_mode=op_mode;  
-    if(this->op_mode==config_mode)
+    if(op_mode==config_mode)
     {
       if(this->thread_server->get_thread_state(this->data_thread_id)==starting ||
         this->thread_server->get_thread_state(this->data_thread_id)==active)
       {
         this->event_server->set_event(this->finish_thread_event_id);
         this->thread_server->end_thread(this->data_thread_id);
+        this->event_server->reset_event(this->finish_thread_event_id);
       }  
+      try{
+        this->imu_access.enter();
+        this->change_register_page(0x00);
+        this->write_registers(0x3D,1,(unsigned char *)&op_mode);
+        this->imu_access.exit();
+      }catch(CException &e){
+        this->imu_access.exit();
+        throw;
+      }
+      this->op_mode=op_mode;  
     } 
     else
     {
-      switch(op_mode)
-      {
-        case config_mode: break;
-        case acc_only_mode: this->raw_mag_data=std::vector<double>(3,0.0);
-                            this->raw_gyro_data=std::vector<double>(3,0.0);
-                            this->quaternion_data=std::vector<double>(3,0.0);
-                            this->euler_angles_data=std::vector<double>(3,0.0);
-                            this->linear_accel_data=std::vector<double>(3,0.0);
-                            this->gravity_data=std::vector<double>(3,0.0);
-                            break;
-        case mag_only_mode: this->raw_accel_data=std::vector<double>(3,0.0);
-                            this->raw_gyro_data=std::vector<double>(3,0.0);
-                            this->quaternion_data=std::vector<double>(3,0.0);
-                            this->euler_angles_data=std::vector<double>(3,0.0);
-                            this->linear_accel_data=std::vector<double>(3,0.0);
-                            this->gravity_data=std::vector<double>(3,0.0);
-                            break;
-        case gyro_only_mode: this->raw_accel_data=std::vector<double>(3,0.0);
-                             this->raw_mag_data=std::vector<double>(3,0.0);
-                             this->quaternion_data=std::vector<double>(3,0.0);
-                             this->euler_angles_data=std::vector<double>(3,0.0);
-                             this->linear_accel_data=std::vector<double>(3,0.0);
-                             this->gravity_data=std::vector<double>(3,0.0);
-                             break;
-        case acc_mag_mode: this->raw_gyro_data=std::vector<double>(3,0.0);
-                           this->quaternion_data=std::vector<double>(3,0.0);
-                           this->euler_angles_data=std::vector<double>(3,0.0);
-                           this->linear_accel_data=std::vector<double>(3,0.0);
-                           this->gravity_data=std::vector<double>(3,0.0);
-                           break;
-        case acc_gyro_mode: this->raw_mag_data=std::vector<double>(3,0.0);
-                            this->quaternion_data=std::vector<double>(3,0.0);
-                            this->euler_angles_data=std::vector<double>(3,0.0);
-                            this->linear_accel_data=std::vector<double>(3,0.0);
-                            this->gravity_data=std::vector<double>(3,0.0);
-                            break;
-        case mag_gyro_mode: this->raw_accel_data=std::vector<double>(3,0.0);
-                            this->quaternion_data=std::vector<double>(3,0.0);
-                            this->euler_angles_data=std::vector<double>(3,0.0);
-                            this->linear_accel_data=std::vector<double>(3,0.0);
-                            this->gravity_data=std::vector<double>(3,0.0);
-                            break;
-        case acc_mag_gyro_mode: this->quaternion_data=std::vector<double>(3,0.0);
-                                this->euler_angles_data=std::vector<double>(3,0.0);
-                                this->linear_accel_data=std::vector<double>(3,0.0);
-                                this->gravity_data=std::vector<double>(3,0.0);
-                                break;
-        case imu_mode: 
-        case compass_mode: 
-        case m4g_mode: 
-        case ndof_off_mode: 
-        case ndof_mode: this->raw_accel_data=std::vector<double>(3,0.0);
-                        this->raw_mag_data=std::vector<double>(3,0.0);
-                        this->raw_gyro_data=std::vector<double>(3,0.0);
-                        break;
-      }  
+      try{
+        this->imu_access.enter();
+        this->change_register_page(0x00);
+        this->write_registers(0x3D,1,(unsigned char *)&op_mode);
+        this->imu_access.exit();
+      }catch(CException &e){
+        this->imu_access.exit();
+        throw;
+      }
+      this->op_mode=op_mode;  
       if(this->thread_server->get_thread_state(this->data_thread_id)==attached)
         this->thread_server->start_thread(this->data_thread_id);
     }
@@ -893,21 +826,13 @@ void CBNO055IMUDriver::get_gyro_config(unsigned char *range,unsigned char *bandw
 /* temperature functions */
 double CBNO055IMUDriver::get_temperature(void)
 {
-  unsigned char temperature;
+  double data;
 
-  try{
-    this->imu_access.enter();
-    this->change_register_page(0x00);
-    this->read_registers(0x34,1,&temperature);
-    this->imu_access.exit();
-    if(this->temp_units==temp_celcius)  
-      return (double)temperature;
-    else
-      return (double)(temperature*=2.0);
-  }catch(CException &e){
-    this->imu_access.exit();
-    throw;
-  }
+  this->data_access.enter();
+  data=this->temperature;
+  this->data_access.exit();
+
+  return data;
 }
 
 /* fused data functions */
@@ -961,28 +886,28 @@ void CBNO055IMUDriver::load_calibration(const std::string &filename)
   unsigned char cal_data[22];
   std::fstream cal_file;
 
-  try{
-    cal_file.open (filename.c_str(), std::ios::in | std::ios::binary);
-    if(cal_file.is_open())
+  cal_file.open (filename.c_str(), std::ios::in | std::ios::binary);
+  if(cal_file.is_open())
+  {
+    cal_file.read((char *)cal_data,22);
+    if(cal_file.gcount()!=22)
+      throw CBNO055IMUException(_HERE_,"Impossibel to read all data. Invalid file format");
+    else
     {
-      cal_file.read((char *)cal_data,22);
-      if(cal_file.gcount()!=22)
-        throw CBNO055IMUException(_HERE_,"Impossibel to read all data. Invalid file format");
-      else
-      {
-        cal_file.close();
+      cal_file.close();
+      try{
         this->imu_access.enter();
         this->change_register_page(0x00);
         this->write_registers(0x55,22,cal_data);
         this->imu_access.exit();
+      }catch(CException &e){
+        this->imu_access.exit();
+        throw;
       }
     }
-    else
-      throw CBNO055IMUException(_HERE_,"Impossible to open the desired file");
-  }catch(CException &e){
-    this->imu_access.exit();
-    throw;
   }
+  else
+    throw CBNO055IMUException(_HERE_,"Impossible to open the desired file");
 }
 
 void CBNO055IMUDriver::save_calibration(const std::string &filename)
@@ -1011,78 +936,46 @@ void CBNO055IMUDriver::save_calibration(const std::string &filename)
 
 bool CBNO055IMUDriver::is_accelerometer_calibrated(void)
 {
-  unsigned char calibration;
+  bool data;
 
-  try{
-    this->imu_access.enter();
-    this->change_register_page(0x00);
-    this->read_registers(0x35,1,&calibration);
-    this->imu_access.exit();
-    if((calibration&0x0C)==0x0C)
-      return true;
-    else
-      return false;
-  }catch(CException &e){
-    this->imu_access.exit();
-    throw;
-  }
+  this->data_access.enter();
+  data=this->accel_calibrated;
+  this->data_access.exit();
+
+  return data;
 }
 
 bool CBNO055IMUDriver::is_magnetometer_calibrated(void)
 {
-  unsigned char calibration;
+  bool data;
 
-  try{
-    this->imu_access.enter();
-    this->change_register_page(0x00);
-    this->read_registers(0x35,1,&calibration);
-    this->imu_access.exit();
-    if((calibration&0x03)==0x03)
-      return true;
-    else
-      return false;
-  }catch(CException &e){
-    this->imu_access.exit();
-    throw;
-  }
+  this->data_access.enter();
+  data=this->mag_calibrated;
+  this->data_access.exit();
+
+  return data;
 }
 
 bool CBNO055IMUDriver::is_gyroscope_calibrated(void)
 {
-  unsigned char calibration;
+  bool data;
 
-  try{
-    this->imu_access.enter();
-    this->change_register_page(0x00);
-    this->read_registers(0x35,1,&calibration);
-    this->imu_access.exit();
-    if((calibration&0x30)==0x30)
-      return true;
-    else
-      return false;
-  }catch(CException &e){
-    this->imu_access.exit();
-    throw;
-  }
+  this->data_access.enter();
+  data=this->gyro_calibrated;
+  this->data_access.exit();
+
+  return data;
 }
 
 bool CBNO055IMUDriver::is_imu_calibrated(void)
 {
-  unsigned char calibration;
+  bool data;
 
-  try{
-    this->imu_access.enter();
-    this->change_register_page(0x00);
-    this->read_registers(0x35,1,&calibration);
-    this->imu_access.exit();
-    if((calibration&0xC0)==0xC0)
-      return true;
-    else
-      return false;
-  }catch(CException &e){
-    this->imu_access.exit();
-    throw;
-  }
+  this->data_access.enter();
+  data=this->system_calibrated;
+  this->data_access.exit();
+
+  return data;
 }
 
 CBNO055IMUDriver::~CBNO055IMUDriver()
diff --git a/src/bno055_imu_driver.h b/src/bno055_imu_driver.h
index 2ddb42401e89e1734b818bbc00eb301ec56f3edf..f69a302197e3e4ca7b5940e4f15d55b9b73686de 100644
--- a/src/bno055_imu_driver.h
+++ b/src/bno055_imu_driver.h
@@ -28,6 +28,11 @@ class CBNO055IMUDriver
     std::string new_data_event_id;
     /* sensor data */
     CMutex data_access;
+    /* calibration data */
+    bool accel_calibrated;
+    bool mag_calibrated;
+    bool gyro_calibrated;
+    bool system_calibrated;
     /* accelerometer data */
     std::vector<double> raw_accel_data;
     TBNO055AccelConfig accel_config;
@@ -39,6 +44,7 @@ class CBNO055IMUDriver
     TBNO055GyroConfig gyro_config;
     /* temperature data */
     temp_units_t temp_units;
+    double temperature;
     /* fusion data */
     std::vector<double> quaternion_data;
     std::vector<double> euler_angles_data;
@@ -53,6 +59,8 @@ class CBNO055IMUDriver
     void convert_euler_angles_data(unsigned char *data);
     void convert_linear_accel_data(unsigned char *data);
     void convert_gravity_data(unsigned char *data);
+    void convert_temperature_data(unsigned char *data);
+    void convert_calibration_data(unsigned char *data);
     void change_register_page(unsigned char page_id);
     void process_data(unsigned char *data);
     static void *data_thread(void *param);
diff --git a/src/bno055_imu_sim.cpp b/src/bno055_imu_sim.cpp
deleted file mode 100644
index d4d0291f9ed77c8c1aee65162985da19c204a45d..0000000000000000000000000000000000000000
--- a/src/bno055_imu_sim.cpp
+++ /dev/null
@@ -1,370 +0,0 @@
-#include "bno055_imu_sim.h"
-#include "bno055_imu_exceptions.h"
-#include "eventexceptions.h"
-#include <iostream>
-#include <fstream>
-#include <math.h>
-
-unsigned char page0_data[]={0xA0,0xFB,0x32,0x0F,// chip ID's
-                            0x08,0x03,0x00,// version ID's
-                            0x00,//page 0 ID
-                            0x00,0x00,0x00,0x00,0x00,0x00,// acceleration data
-                            0x00,0x00,0x00,0x00,0x00,0x00,// magnetometer data
-                            0x00,0x00,0x00,0x00,0x00,0x00,// gyroscope data
-                            0x00,0x00,0x00,0x00,0x00,0x00,// euler data
-                            0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,// quaternion data
-                            0x00,0x00,0x00,0x00,0x00,0x00,// linear acceleration data
-                            0x00,0x00,0x00,0x00,0x00,0x00,// gravity data
-                            0x00, // temperature
-                            0x00, // calibration status
-                            0x0F, // ST result
-                            0x00, // interrupt status
-                            0x00, // clock status
-                            0x00, // system status
-                            0x00, // system error code
-                            0x80, // units
-                            0xFF, // reserved
-                            0x1C, // operation mode
-                            0x00, // power mode
-                            0x00, // system trigger
-                            0x02, // temperature source
-                            0x00, // axis map config
-                            0x00, // axis map sign
-                            0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-                            0x00,0x00,0x00,0x00,0x00,0x00,// acceleration offset
-                            0x00,0x00,0x00,0x00,0x00,0x00,// magnetometer offset
-                            0x00,0x00,0x00,0x00,0x00,0x00,// gyroscope offset
-                            0x00,0x00,// accelerometer radius
-                            0x00,0x00};// magnetometer radius
-
-unsigned char page1_data[]={0x00,0x00,0x00,0x00,0x00,0x00,0x00, //reserved
-                            0x01, // page ID
-                            0x0D, // accelerometer config
-                            0x6D, // magnetometer config
-                            0x38,0x00, // gyroscope config
-                            0x00, // accelerometer sleep config
-                            0x00, // gyroscope sleep config
-                            0x00, // reserved
-                            0x00, // interrupt mask
-                            0x00, // interrupt enable
-                            0x14, // accelerometer any motion threshold
-                            0x03, // accelerometer interrupt settings
-                            0x0F, // accelerometer High G duration 
-                            0xC0, // accelerometer High G threshold
-                            0x0A, // accelerometer no/slow motion threshold 
-                            0x0B, // accelerometer no/slow motion duration
-                            0x00, // gyroscope interrupt settings
-                            0x01, // gyroscope high rate thresold X
-                            0x19, // gyroscope high rate duration X
-                            0x01, // gyroscope high rate thresold Y
-                            0x19, // gyroscope high rate duration Y
-                            0x01, // gyroscope high rate thresold Z
-                            0x19, // gyroscope high rate duration Z
-                            0x04, // gyroscope any motion threshold
-                            0x0A, // gyroscope awake duration
-                            0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-                            0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-                            0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-                            0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,0x0B,0x0C,0x0D,0x0E,0x0F};// BNO unique ID
-
-typedef enum {imu_header,imu_cmd,imu_address,imu_length,imu_data} receive_states_t;
-
-CBNO055IMUSim::CBNO055IMUSim(const std::string &name,const std::string &serial_dev)
-{
-  TRS232_config config;
-
-  if(name.size()==0)
-    throw CBNO055IMUException(_HERE_,"Invalid IMU name");
-  else
-  {
-    this->imu_name=name;
-    this->imu_port=NULL;
-    this->imu_device.clear();
-    this->power_mode=normal_mode;
-    this->op_mode=config_mode;
-    this->current_page=0x00;
-    try{
-      this->imu_port=new CRS232(this->imu_name+"_serial_port");
-      this->imu_port->open((void *)&serial_dev);
-      config.baud=115200;
-      config.num_bits=8;
-      config.parity=none;
-      config.stop_bits=1;
-      this->imu_port->config(&config);
-    }catch(CException &s){
-      if(this->imu_port!=NULL)
-      {
-        this->imu_port->close();
-        delete this->imu_port;
-        this->imu_port=NULL;
-      }
-      throw;
-    }
-    /* initialize events */
-    this->event_server=CEventServer::instance();
-    this->finish_thread_event_id=this->imu_name+"_finish_thread_event";
-    this->event_server->create_event(this->finish_thread_event_id);
-    /* initialize threads */
-    this->thread_server=CThreadServer::instance();
-    this->data_thread_id=this->imu_name+"_data_thread";
-    this->thread_server->create_thread(this->data_thread_id);
-    this->thread_server->attach_thread(this->data_thread_id,this->data_thread,this);
-    this->thread_server->start_thread(this->data_thread_id);
-  }
-}
-
-void *CBNO055IMUSim::data_thread(void *param)
-{
-  CBNO055IMUSim *imu=(CBNO055IMUSim *)param;
-  receive_states_t state=imu_header;
-  unsigned char address,length,op,*data,*serial_data,*cmd;
-  std::list<std::string> events;
-  unsigned int index,num,num_read=0,i,j;
-  bool end=false;
-
-  events.push_back(imu->finish_thread_event_id);
-  events.push_back(imu->imu_port->get_rx_event_id());
-
-  while(!end)
-  {
-    index=imu->event_server->wait_first(events);
-    if(index==0)
-      end=true;
-    else if(index==1)
-    {
-      imu->data_access.enter();
-      num=imu->imu_port->get_num_data();
-      serial_data=new unsigned char[num];
-      imu->imu_port->read(serial_data,num);
-      for(i=0;i<num;i++)
-      {
-        switch(state)
-        {
-          case imu_header: if(serial_data[i]==0xAA)
-                             state=imu_cmd;
-                           else
-                             state=imu_header;
-                           break;
-          case imu_cmd: state=imu_address;
-                        op=serial_data[i];
-                        break;
-          case imu_address: state=imu_length;
-                            address=serial_data[i];
-                            break;
-          case imu_length: length=serial_data[i]; 
-                           if(op==0x00)
-                           {
-                             state=imu_data;
-                             data=new unsigned char[length];
-                             num_read=0;
-                           }
-                           else
-                           {
-                             state= imu_header;
-                             // send the desired data
-                             cmd=new unsigned char[2+length];
-                             cmd[0]=0xBB;
-                             cmd[1]=length; 
-                             for(j=0;j<length;j++)
-                             {
-                               if(imu->current_page==0x00)
-                                 cmd[2+j]=page0_data[address+j];
-                               else
-                                 cmd[2+j]=page1_data[address+j];
-                             }
-                             imu->imu_port->write(cmd,2+length);
-                             delete[] cmd;
-                           }
-                           break;
-          case imu_data: data[num_read]=serial_data[i];
-                         num_read++;
-                         if(num_read==length)
-                         {
-                           // process the write command
-                           // send the response
-                           cmd=new unsigned char[2];
-                           cmd[0]=0xBB;
-                           cmd[1]=0x01;
-                           imu->imu_port->write(cmd,2);
-                           delete[] cmd;
-                           delete[] data;
-                           state=imu_header;
-                         }                         
-                         else
-                           state=imu_data;
-                         break;
-        }
-      }
-      delete[] serial_data;
-      imu->data_access.exit();
-    }
-  }
-
-  pthread_exit(NULL);
-}
-
-void CBNO055IMUSim::process_write(unsigned char address, unsigned char length, unsigned char *data)
-{
-  unsigned int i=0;
-
-  for(i=address;i<address+length;i++)
-  {
-    if(this->current_page==0x00)
-    {
-      page0_data[i]=data[i-address];
-      switch(i)
-      {
-        case 0x07: this->current_page=data[i-address];
-                   break;
-        case 0x3B: if(data[i-address]&0x01)
-                     this->accel_config.units=accel_gravity;
-                   else
-                     this->accel_config.units=accel_linear;
-                   if(data[i-address]&0x02)
-                     this->gyro_config.units=gyro_rad;
-                   else
-                     this->gyro_config.units=gyro_deg;
-                   if(data[i-address]&0x04)
-                     this->euler_units=euler_rad;
-                   else
-                     this->euler_units=euler_deg;
-                   if(data[i-address]&0x10)
-                     this->temp_units=temp_farenheit;
-                   else
-                     this->temp_units=temp_celcius;
-                   break;
-      }
-    }
-    else
-    {
-      switch(i)
-      {
-        case 0x07: this->current_page=data[i-address];
-                   break;
-      }
-    }
-  }
-}
-
-void CBNO055IMUSim::set_calibrated(void)
-{
-  page0_data[53]=0xFF;
-}
-
-void CBNO055IMUSim::set_accel_data(double x, double y, double z)
-{
-  page0_data[8]=((int)(x*100.0))%256;
-  page0_data[9]=((int)(x*100.0))/256;
-  page0_data[10]=((int)(y*100.0))%256;
-  page0_data[11]=((int)(y*100.0))/256;
-  page0_data[12]=((int)(z*100.0))%256;
-  page0_data[13]=((int)(z*100.0))/256;
-}
-
-void CBNO055IMUSim::set_gyro_data(double x, double y, double z)
-{
-  if(this->gyro_config.units==gyro_deg)
-  {
-    page0_data[20]=((int)(x*16.0))%256;
-    page0_data[21]=((int)(x*16.0))/256;
-    page0_data[22]=((int)(y*16.0))%256;
-    page0_data[23]=((int)(y*16.0))/256;
-    page0_data[24]=((int)(z*16.0))%256;
-    page0_data[25]=((int)(z*16.0))/256;
-  }
-  else
-  {
-    page0_data[20]=((int)(x*900.0))%256;
-    page0_data[21]=((int)(x*900.0))/256;
-    page0_data[22]=((int)(y*900.0))%256;
-    page0_data[23]=((int)(y*900.0))/256;
-    page0_data[24]=((int)(z*900.0))%256;
-    page0_data[25]=((int)(z*900.0))/256;
-  }
-}
-
-void CBNO055IMUSim::set_mag_data(double x, double y, double z)
-{
-  page0_data[14]=((int)(x*16.0))%256;
-  page0_data[15]=((int)(x*16.0))/256;
-  page0_data[16]=((int)(y*16.0))%256;
-  page0_data[17]=((int)(y*16.0))/256;
-  page0_data[18]=((int)(z*16.0))%256;
-  page0_data[19]=((int)(z*16.0))/256;
-}
-
-void CBNO055IMUSim::set_quaternion_data(double x, double y, double z,double w)
-{
-  page0_data[32]=((int)(w*16384.0))%256;
-  page0_data[33]=((int)(w*16384.0))/256;
-  page0_data[34]=((int)(x*16384.0))%256;
-  page0_data[35]=((int)(x*16384.0))/256;
-  page0_data[36]=((int)(y*16384.0))%256;
-  page0_data[37]=((int)(y*16384.0))/256;
-  page0_data[38]=((int)(z*16384.0))%256;
-  page0_data[39]=((int)(z*16384.0))/256;
-}
-
-void CBNO055IMUSim::set_euler_angles_data(double yaw, double roll, double pitch)
-{
-  if(this->euler_units==euler_deg)
-  {
-    page0_data[26]=((int)(yaw*16.0))%256;
-    page0_data[27]=((int)(yaw*16.0))/256;
-    page0_data[28]=((int)(roll*16.0))%256;
-    page0_data[29]=((int)(roll*16.0))/256;
-    page0_data[30]=((int)(pitch*16.0))%256;
-    page0_data[31]=((int)(pitch*16.0))/256;
-  }
-  else
-  {
-    page0_data[26]=((int)(yaw*900.0))%256;
-    page0_data[27]=((int)(yaw*900.0))/256;
-    page0_data[28]=((int)(roll*900.0))%256;
-    page0_data[29]=((int)(roll*900.0))/256;
-    page0_data[30]=((int)(pitch*900.0))%256;
-    page0_data[31]=((int)(pitch*900.0))/256;
-  }
-}
-
-void CBNO055IMUSim::set_linear_accel_data(double x, double y, double z)
-{
-  page0_data[40]=((int)(x*100.0))%256;
-  page0_data[41]=((int)(x*100.0))/256;
-  page0_data[42]=((int)(y*100.0))%256;
-  page0_data[43]=((int)(y*100.0))/256;
-  page0_data[44]=((int)(z*100.0))%256;
-  page0_data[45]=((int)(z*100.0))/256;
-}
-
-void CBNO055IMUSim::set_gravity_data(double x, double y, double z)
-{
-  page0_data[46]=((int)(x*100.0))%256;
-  page0_data[47]=((int)(x*100.0))/256;
-  page0_data[48]=((int)(y*100.0))%256;
-  page0_data[49]=((int)(y*100.0))/256;
-  page0_data[50]=((int)(z*100.0))%256;
-  page0_data[51]=((int)(z*100.0))/256;
-}
-
-CBNO055IMUSim::~CBNO055IMUSim()
-{
-  if(this->thread_server->get_thread_state(this->data_thread_id)==starting ||
-     this->thread_server->get_thread_state(this->data_thread_id)==active)
-  {
-    this->event_server->set_event(this->finish_thread_event_id);
-    this->thread_server->end_thread(this->data_thread_id);
-  }
-  this->thread_server->detach_thread(this->data_thread_id);
-  this->thread_server->delete_thread(this->data_thread_id);
-  this->data_thread_id="";
-  this->event_server->delete_event(this->finish_thread_event_id);
-  this->finish_thread_event_id="";
-  if(this->imu_port!=NULL)
-  {
-    this->imu_port->close();
-    delete this->imu_port;
-    this->imu_port=NULL;
-  }
-}
-
diff --git a/src/bno055_imu_sim.h b/src/bno055_imu_sim.h
deleted file mode 100644
index 0628c3265c87957a524b5f4e9b021b2aa7416219..0000000000000000000000000000000000000000
--- a/src/bno055_imu_sim.h
+++ /dev/null
@@ -1,54 +0,0 @@
-#ifndef _BNO055_IMU_SIM_H
-#define _BNO055_IMU_SIM_H
-
-#include "rs232.h"
-#include "mutex.h"
-#include "threadserver.h"
-#include "bno055_common.h"
-#include <vector>
-
-class CBNO055IMUSim
-{
-  private:
-    std::string imu_name;
-    CRS232 *imu_port;
-    std::string imu_device;
-    CMutex imu_access;
-    power_mode_t power_mode;
-    op_mode_t op_mode;
-    unsigned char current_page;
-    /* thread attributes */
-    CThreadServer *thread_server;
-    std::string data_thread_id;
-    /* event attributes */
-    CEventServer *event_server;
-    std::string finish_thread_event_id;
-    /* sensor data */
-    CMutex data_access;
-    /* accelerometer data */
-    TBNO055AccelConfig accel_config;
-    /* magnetometer data */
-    TBNO055MagConfig mag_config;
-    /* gyroscope data */
-    TBNO055GyroConfig gyro_config;
-    /* temperature data */
-    temp_units_t temp_units;
-    /* fusion data */
-    euler_units_t euler_units;
-  protected:
-    static void *data_thread(void *param);
-    void process_write(unsigned char address, unsigned char length, unsigned char *data);
-  public:
-    CBNO055IMUSim(const std::string &name,const std::string &serial_dev);
-    void set_calibrated(void);
-    void set_accel_data(double x, double y, double z);
-    void set_gyro_data(double x, double y, double z);
-    void set_mag_data(double x, double y, double z);
-    void set_quaternion_data(double x, double y, double z,double w);
-    void set_euler_angles_data(double yaw, double roll, double pitch);
-    void set_linear_accel_data(double x, double y, double z);
-    void set_gravity_data(double x, double y, double z);
-    ~CBNO055IMUSim();
-};
-
-#endif
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 586bfd62897e2bfbc341b5ce3d6b2b20c68da8e1..fd72ae5a9b0a36a24c83a0ca340577a11a071117 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -2,8 +2,3 @@
 ADD_EXECUTABLE(bno055_imu_driver_test bno055_imu_driver_test.cpp)
 # link necessary libraries
 TARGET_LINK_LIBRARIES(bno055_imu_driver_test bno055_imu_driver)
-
-# create an example application
-ADD_EXECUTABLE(bno055_imu_sim_test bno055_imu_sim_test.cpp)
-# link necessary libraries
-TARGET_LINK_LIBRARIES(bno055_imu_sim_test bno055_imu_sim)
diff --git a/src/examples/bno055_imu_driver_test.cpp b/src/examples/bno055_imu_driver_test.cpp
index d5722fe25b428aa81feaefa8f232943e817432f0..55498fcd4009fffb7b9e394d201c9f0e561e43f2 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("/tmp/darwin1_imu_uart");
     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){
diff --git a/src/examples/bno055_imu_sim_test.cpp b/src/examples/bno055_imu_sim_test.cpp
deleted file mode 100644
index 1c783f49ffd44956620e0b20de567b1ea486fd85..0000000000000000000000000000000000000000
--- a/src/examples/bno055_imu_sim_test.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-#include "bno055_imu_sim.h"
-#include "bno055_imu_exceptions.h"
-#include <unistd.h>
-#include <iostream>
-
-int main(int argc, char *argv[])
-{
-  try{
-    CBNO055IMUSim imu("darwin_imu","/dev/pts/23");
-
-    sleep(3);
-    imu.set_calibrated();
-
-    while(1);
-
-  }catch(CException &e){
-    std::cout << e.what() << std::endl;
-  }
-}