From 95a9b8fadf37b291c5fef33ca3d13a740296d299 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Mon, 3 Feb 2020 15:24:17 +0100
Subject: [PATCH] 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 | 31 ++++++++++++++++---------------
1 file changed, 16 insertions(+), 15 deletions(-)
diff --git a/src/bno055_imu_driver.cpp b/src/bno055_imu_driver.cpp
index bcf9899..c140a39 100644
--- a/src/bno055_imu_driver.cpp
+++ b/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)
--
GitLab