Merge branch 'master' of ssh://gitlab.iri.upc.edu:2202/labrobotica/drivers/bno055_imu_driver

.gitignore

+bin
+lib
+build

Findbno055_imu_sim.cmake

-#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)
-

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)

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()

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);

src/bno055_imu_sim.cpp

-#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