Solved a bug in the accel_gyro mode: the gyro data was not properly updated.

Solved a bug in the load_calibration function: the mutex was unlocked without a previous lock if the file didn't exist.

src/bno055_imu_driver.cpp

@@ -162,7 +162,7 @@ void CBNO055IMUDriver::process_data(unsigned char *data)
                        this->convert_mag_data(&data[6]);
 		       break;
     case acc_gyro_mode: this->convert_accel_data(data);
-                        this->convert_gyro_data(&data[6]);
+                        this->convert_gyro_data(&data[12]);
 			break;
     case mag_gyro_mode: this->convert_mag_data(data);
                         this->convert_gyro_data(&data[6]);
@@ -253,6 +253,7 @@ void *CBNO055IMUDriver::data_thread(void *param)
           }
         }catch(CException &e){
           imu->imu_access.exit();
+          delete[] data;
           std::cout << e.what() << std::endl;
         }
       }
@@ -961,28 +962,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)