Commit 95a9b8fa authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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.
parent 00e721bc
......@@ -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)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment