diff --git a/src/bno055_imu_driver.cpp b/src/bno055_imu_driver.cpp index bcf989973dd2a01b5e7dce36743e2cc2544eee03..c140a3937469349a2dcde04d06372766f753ffb7 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)