diff --git a/calibration/bno055.cal b/calibration/bno055.cal
new file mode 100644
index 0000000000000000000000000000000000000000..415824d33b03a35d9afda9fda66eb71dd64af48e
Binary files /dev/null and b/calibration/bno055.cal differ
diff --git a/src/bno055_imu_driver.cpp b/src/bno055_imu_driver.cpp
index 70c415f96a378b3a9e15bdda24866164dcb5d697..bcf989973dd2a01b5e7dce36743e2cc2544eee03 100644
--- a/src/bno055_imu_driver.cpp
+++ b/src/bno055_imu_driver.cpp
@@ -5,7 +5,7 @@
#include <fstream>
#include <math.h>
-BNO055IMUDriver::BNO055IMUDriver(const std::string &name)
+CBNO055IMUDriver::CBNO055IMUDriver(const std::string &name)
{
if(name.size()==0)
throw CBNO055IMUException(_HERE_,"Invalid IMU name");
@@ -46,7 +46,7 @@ BNO055IMUDriver::BNO055IMUDriver(const std::string &name)
}
}
-void BNO055IMUDriver::convert_accel_data(unsigned char *data)
+void CBNO055IMUDriver::convert_accel_data(unsigned char *data)
{
this->raw_accel_data[0]=(signed short int)(data[0]+(data[1]<<8));
this->raw_accel_data[1]=(signed short int)(data[2]+(data[3]<<8));
@@ -59,14 +59,14 @@ void BNO055IMUDriver::convert_accel_data(unsigned char *data)
}
}
-void BNO055IMUDriver::convert_mag_data(unsigned char *data)
+void CBNO055IMUDriver::convert_mag_data(unsigned char *data)
{
this->raw_mag_data[0]=((signed short int)(data[0]+(data[1]<<8)))/16.0;
this->raw_mag_data[1]=((signed short int)(data[2]+(data[3]<<8)))/16.0;
this->raw_mag_data[2]=((signed short int)(data[4]+(data[5]<<8)))/16.0;
}
-void BNO055IMUDriver::convert_gyro_data(unsigned char *data)
+void CBNO055IMUDriver::convert_gyro_data(unsigned char *data)
{
if(this->gyro_config.units==gyro_deg)
{
@@ -82,7 +82,7 @@ void BNO055IMUDriver::convert_gyro_data(unsigned char *data)
}
}
-void BNO055IMUDriver::convert_quaternion_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;
@@ -90,7 +90,7 @@ void BNO055IMUDriver::convert_quaternion_data(unsigned char *data)
this->quaternion_data[2]=((signed short int)(data[6]+(data[7]<<8)))/16384.0;
}
-void BNO055IMUDriver::convert_euler_angles_data(unsigned char *data)
+void CBNO055IMUDriver::convert_euler_angles_data(unsigned char *data)
{
if(this->euler_units==euler_deg)
{
@@ -106,7 +106,7 @@ void BNO055IMUDriver::convert_euler_angles_data(unsigned char *data)
}
}
-void BNO055IMUDriver::convert_linear_accel_data(unsigned char *data)
+void CBNO055IMUDriver::convert_linear_accel_data(unsigned char *data)
{
this->linear_accel_data[0]=(signed short int)(data[0]+(data[1]<<8));
this->linear_accel_data[1]=(signed short int)(data[2]+(data[3]<<8));
@@ -119,7 +119,7 @@ void BNO055IMUDriver::convert_linear_accel_data(unsigned char *data)
}
}
-void BNO055IMUDriver::convert_gravity_data(unsigned char *data)
+void CBNO055IMUDriver::convert_gravity_data(unsigned char *data)
{
this->gravity_data[0]=(signed short int)(data[0]+(data[1]<<8));
this->gravity_data[1]=(signed short int)(data[2]+(data[3]<<8));
@@ -132,7 +132,7 @@ void BNO055IMUDriver::convert_gravity_data(unsigned char *data)
}
}
-void BNO055IMUDriver::change_register_page(unsigned char page_id)
+void CBNO055IMUDriver::change_register_page(unsigned char page_id)
{
if(page_id!=0x01 && page_id!=0x00)
throw CBNO055IMUException(_HERE_,"Invalid register page ID");
@@ -146,7 +146,7 @@ void BNO055IMUDriver::change_register_page(unsigned char page_id)
}
}
-void BNO055IMUDriver::process_data(unsigned char *data)
+void CBNO055IMUDriver::process_data(unsigned char *data)
{
this->data_access.enter();
switch(this->op_mode)
@@ -184,9 +184,9 @@ void BNO055IMUDriver::process_data(unsigned char *data)
this->data_access.exit();
}
-void *BNO055IMUDriver::data_thread(void *param)
+void *CBNO055IMUDriver::data_thread(void *param)
{
- BNO055IMUDriver *imu=(BNO055IMUDriver *)param;
+ CBNO055IMUDriver *imu=(CBNO055IMUDriver *)param;
unsigned char address,length,*data;
bool end=false;
@@ -263,7 +263,7 @@ void *BNO055IMUDriver::data_thread(void *param)
pthread_exit(NULL);
}
-void BNO055IMUDriver::write_registers(unsigned char address, unsigned char length,unsigned char *data)
+void CBNO055IMUDriver::write_registers(unsigned char address, unsigned char length,unsigned char *data)
{
unsigned char *cmd,answer[2];
std::list<std::string> events;
@@ -287,7 +287,7 @@ void BNO055IMUDriver::write_registers(unsigned char address, unsigned char lengt
{
if((num=this->imu_port->get_num_data())==0)
{
- this->event_server->wait_all(events,5000);
+ this->event_server->wait_all(events,500);
num=this->imu_port->get_num_data();
}
if(num>(2-read_data))
@@ -324,7 +324,7 @@ void BNO055IMUDriver::write_registers(unsigned char address, unsigned char lengt
}
-void BNO055IMUDriver::read_registers(unsigned char address, unsigned char length,unsigned char *data)
+void CBNO055IMUDriver::read_registers(unsigned char address, unsigned char length,unsigned char *data)
{
unsigned char cmd[4],answer[2];
std::list<std::string> events;
@@ -346,7 +346,7 @@ void BNO055IMUDriver::read_registers(unsigned char address, unsigned char length
{
if((num=this->imu_port->get_num_data())==0)
{
- this->event_server->wait_all(events,5000);
+ this->event_server->wait_all(events,500);
num=this->imu_port->get_num_data();
}
if(num>(2-read_data))
@@ -386,7 +386,7 @@ void BNO055IMUDriver::read_registers(unsigned char address, unsigned char length
{
if((num=this->imu_port->get_num_data())==0)
{
- this->event_server->wait_all(events,5000);
+ this->event_server->wait_all(events,500);
num=this->imu_port->get_num_data();
}
if(num>(length-read_data))
@@ -407,7 +407,7 @@ void BNO055IMUDriver::read_registers(unsigned char address, unsigned char length
}
}
-void BNO055IMUDriver::detect_sensor(void)
+void CBNO055IMUDriver::detect_sensor(void)
{
try{
this->imu_access.enter();
@@ -422,7 +422,7 @@ void BNO055IMUDriver::detect_sensor(void)
}
}
-void BNO055IMUDriver::get_sensor_units(void)
+void CBNO055IMUDriver::get_sensor_units(void)
{
unsigned char units;
@@ -442,7 +442,7 @@ void BNO055IMUDriver::get_sensor_units(void)
}
}
-void BNO055IMUDriver::get_sensor_configs(void)
+void CBNO055IMUDriver::get_sensor_configs(void)
{
unsigned char configs[4];
@@ -461,7 +461,7 @@ void BNO055IMUDriver::get_sensor_configs(void)
}
}
-void BNO055IMUDriver::open(const std::string &serial_dev)
+void CBNO055IMUDriver::open(const std::string &serial_dev)
{
TRS232_config config;
@@ -477,6 +477,7 @@ void BNO055IMUDriver::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->op_mode=this->get_operation_mode();
this->set_operation_mode(config_mode);
this->power_mode=this->get_power_mode();
this->get_sensor_units();
@@ -492,7 +493,7 @@ void BNO055IMUDriver::open(const std::string &serial_dev)
}
}
-void BNO055IMUDriver::set_power_mode(power_mode_t power_mode)
+void CBNO055IMUDriver::set_power_mode(power_mode_t power_mode)
{
if(this->op_mode==config_mode)
{
@@ -511,7 +512,7 @@ void BNO055IMUDriver::set_power_mode(power_mode_t power_mode)
throw CBNO055IMUException(_HERE_,"Impossible to change the configuration when not in the config mode");
}
-power_mode_t BNO055IMUDriver::get_power_mode(void)
+power_mode_t CBNO055IMUDriver::get_power_mode(void)
{
unsigned char power_mode;
@@ -528,91 +529,94 @@ power_mode_t BNO055IMUDriver::get_power_mode(void)
return (power_mode_t)power_mode;
}
-void BNO055IMUDriver::set_operation_mode(op_mode_t op_mode)
+void CBNO055IMUDriver::set_operation_mode(op_mode_t 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(this->op_mode!=op_mode)
{
- if(this->thread_server->get_thread_state(this->data_thread_id)==starting ||
- this->thread_server->get_thread_state(this->data_thread_id)==active)
+ 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)
{
- this->event_server->set_event(this->finish_thread_event_id);
- this->thread_server->end_thread(this->data_thread_id);
- }
- }
- else
- {
- switch(op_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);
+ }
+ }
+ else
{
- 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);
+ 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_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;
- }
- if(this->thread_server->get_thread_state(this->data_thread_id)==attached)
- this->thread_server->start_thread(this->data_thread_id);
+ 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;
+ }
+ if(this->thread_server->get_thread_state(this->data_thread_id)==attached)
+ this->thread_server->start_thread(this->data_thread_id);
+ }
}
}
-op_mode_t BNO055IMUDriver::get_operation_mode(void)
+op_mode_t CBNO055IMUDriver::get_operation_mode(void)
{
unsigned char op_mode;
@@ -629,7 +633,7 @@ op_mode_t BNO055IMUDriver::get_operation_mode(void)
return (op_mode_t)op_mode;
}
-void BNO055IMUDriver::set_remap_axis(std::vector<char> &new_x,std::vector<char> &new_y,std::vector<char> &new_z)
+void CBNO055IMUDriver::set_remap_axis(std::vector<char> &new_x,std::vector<char> &new_y,std::vector<char> &new_z)
{
unsigned char remapping[2]={0x00,0x00};
unsigned int i,num_coef;
@@ -711,7 +715,7 @@ void BNO055IMUDriver::set_remap_axis(std::vector<char> &new_x,std::vector<char>
throw CBNO055IMUException(_HERE_,"Impossible to change the configuration when not in the config mode");
}
-void BNO055IMUDriver::get_remap_axis(std::vector<char> &new_x,std::vector<char> &new_y,std::vector<char> &new_z)
+void CBNO055IMUDriver::get_remap_axis(std::vector<char> &new_x,std::vector<char> &new_y,std::vector<char> &new_z)
{
unsigned char remapping[2];
@@ -742,7 +746,7 @@ void BNO055IMUDriver::get_remap_axis(std::vector<char> &new_x,std::vector<char>
}
}
-void BNO055IMUDriver::set_data_rate(double rate_hz)
+void CBNO055IMUDriver::set_data_rate(double rate_hz)
{
if(rate_hz>0.0 && rate_hz<=100.0)
this->data_rate_hz=rate_hz;
@@ -751,18 +755,18 @@ void BNO055IMUDriver::set_data_rate(double rate_hz)
}
-double BNO055IMUDriver::get_data_rate(void)
+double CBNO055IMUDriver::get_data_rate(void)
{
return this->data_rate_hz;
}
-std::string BNO055IMUDriver::get_new_data_event_id(void)
+std::string CBNO055IMUDriver::get_new_data_event_id(void)
{
return this->new_data_event_id;
}
/* accelerometer functions */
-std::vector<double> BNO055IMUDriver::get_raw_accelerometer(void)
+std::vector<double> CBNO055IMUDriver::get_raw_accelerometer(void)
{
std::vector<double> data;
@@ -773,7 +777,7 @@ std::vector<double> BNO055IMUDriver::get_raw_accelerometer(void)
return data;
}
-void BNO055IMUDriver::set_accel_config(unsigned char range,unsigned char bandwidth, unsigned char mode)
+void CBNO055IMUDriver::set_accel_config(unsigned char range,unsigned char bandwidth, unsigned char mode)
{
unsigned char config;
@@ -795,7 +799,7 @@ void BNO055IMUDriver::set_accel_config(unsigned char range,unsigned char bandwid
throw CBNO055IMUException(_HERE_,"Impossible to change the configuration when not in the config mode");
}
-void BNO055IMUDriver::get_accel_config(unsigned char *range,unsigned char *bandwidth, unsigned char *mode)
+void CBNO055IMUDriver::get_accel_config(unsigned char *range,unsigned char *bandwidth, unsigned char *mode)
{
(*range)=this->accel_config.conf_reg&ACCEL_RANGE_MASK;
(*bandwidth)=this->accel_config.conf_reg&ACCEL_BW_MASK;
@@ -803,7 +807,7 @@ void BNO055IMUDriver::get_accel_config(unsigned char *range,unsigned char *bandw
}
/* magnetometer functions */
-std::vector<double> BNO055IMUDriver::get_raw_magnetometer(void)
+std::vector<double> CBNO055IMUDriver::get_raw_magnetometer(void)
{
std::vector<double> data;
@@ -814,7 +818,7 @@ std::vector<double> BNO055IMUDriver::get_raw_magnetometer(void)
return data;
}
-void BNO055IMUDriver::set_mag_config(unsigned char rate,unsigned char op_mode, unsigned char power_mode)
+void CBNO055IMUDriver::set_mag_config(unsigned char rate,unsigned char op_mode, unsigned char power_mode)
{
unsigned char config;
@@ -836,7 +840,7 @@ void BNO055IMUDriver::set_mag_config(unsigned char rate,unsigned char op_mode, u
throw CBNO055IMUException(_HERE_,"Impossible to change the configuration when not in the config mode");
}
-void BNO055IMUDriver::get_mag_config(unsigned char *rate,unsigned char *op_mode, unsigned char *power_mode)
+void CBNO055IMUDriver::get_mag_config(unsigned char *rate,unsigned char *op_mode, unsigned char *power_mode)
{
(*rate)=this->mag_config.conf_reg&MAG_RATE_MASK;
(*op_mode)=this->mag_config.conf_reg&MAG_OP_MODE_MASK;
@@ -844,7 +848,7 @@ void BNO055IMUDriver::get_mag_config(unsigned char *rate,unsigned char *op_mode,
}
/* gyroscope functions */
-std::vector<double> BNO055IMUDriver::get_raw_gyroscope(void)
+std::vector<double> CBNO055IMUDriver::get_raw_gyroscope(void)
{
std::vector<double> data;
@@ -855,7 +859,7 @@ std::vector<double> BNO055IMUDriver::get_raw_gyroscope(void)
return data;
}
-void BNO055IMUDriver::set_gyro_config(unsigned char range,unsigned char bandwidth, unsigned char mode)
+void CBNO055IMUDriver::set_gyro_config(unsigned char range,unsigned char bandwidth, unsigned char mode)
{
unsigned char config[2];
@@ -879,7 +883,7 @@ void BNO055IMUDriver::set_gyro_config(unsigned char range,unsigned char bandwidt
throw CBNO055IMUException(_HERE_,"Impossible to change the configuration when not in the config mode");
}
-void BNO055IMUDriver::get_gyro_config(unsigned char *range,unsigned char *bandwidth, unsigned char *mode)
+void CBNO055IMUDriver::get_gyro_config(unsigned char *range,unsigned char *bandwidth, unsigned char *mode)
{
(*range)=this->gyro_config.conf_reg1&GYRO_RANGE_MASK;
(*bandwidth)=this->gyro_config.conf_reg1&GYRO_BW_MASK;
@@ -887,7 +891,7 @@ void BNO055IMUDriver::get_gyro_config(unsigned char *range,unsigned char *bandwi
}
/* temperature functions */
-double BNO055IMUDriver::get_temperature(void)
+double CBNO055IMUDriver::get_temperature(void)
{
unsigned char temperature;
@@ -907,7 +911,7 @@ double BNO055IMUDriver::get_temperature(void)
}
/* fused data functions */
-std::vector<double> BNO055IMUDriver::get_orientation_quat(void)
+std::vector<double> CBNO055IMUDriver::get_orientation_quat(void)
{
std::vector<double> data;
@@ -918,7 +922,7 @@ std::vector<double> BNO055IMUDriver::get_orientation_quat(void)
return data;
}
-std::vector<double> BNO055IMUDriver::get_orientation_euler(void)
+std::vector<double> CBNO055IMUDriver::get_orientation_euler(void)
{
std::vector<double> data;
@@ -929,7 +933,7 @@ std::vector<double> BNO055IMUDriver::get_orientation_euler(void)
return data;
}
-std::vector<double> BNO055IMUDriver::get_linear_accel(void)
+std::vector<double> CBNO055IMUDriver::get_linear_accel(void)
{
std::vector<double> data;
@@ -940,7 +944,7 @@ std::vector<double> BNO055IMUDriver::get_linear_accel(void)
return data;
}
-std::vector<double> BNO055IMUDriver::get_gravity(void)
+std::vector<double> CBNO055IMUDriver::get_gravity(void)
{
std::vector<double> data;
@@ -952,7 +956,7 @@ std::vector<double> BNO055IMUDriver::get_gravity(void)
}
/* calibration functions */
-void BNO055IMUDriver::load_calibration(const std::string &filename)
+void CBNO055IMUDriver::load_calibration(const std::string &filename)
{
unsigned char cal_data[22];
std::fstream cal_file;
@@ -981,7 +985,7 @@ void BNO055IMUDriver::load_calibration(const std::string &filename)
}
}
-void BNO055IMUDriver::save_calibration(const std::string &filename)
+void CBNO055IMUDriver::save_calibration(const std::string &filename)
{
unsigned char cal_data[22];
std::fstream cal_file;
@@ -1005,7 +1009,7 @@ void BNO055IMUDriver::save_calibration(const std::string &filename)
}
}
-bool BNO055IMUDriver::is_accelerometer_calibrated(void)
+bool CBNO055IMUDriver::is_accelerometer_calibrated(void)
{
unsigned char calibration;
@@ -1024,7 +1028,7 @@ bool BNO055IMUDriver::is_accelerometer_calibrated(void)
}
}
-bool BNO055IMUDriver::is_magnetometer_calibrated(void)
+bool CBNO055IMUDriver::is_magnetometer_calibrated(void)
{
unsigned char calibration;
@@ -1043,7 +1047,7 @@ bool BNO055IMUDriver::is_magnetometer_calibrated(void)
}
}
-bool BNO055IMUDriver::is_gyroscope_calibrated(void)
+bool CBNO055IMUDriver::is_gyroscope_calibrated(void)
{
unsigned char calibration;
@@ -1062,7 +1066,7 @@ bool BNO055IMUDriver::is_gyroscope_calibrated(void)
}
}
-bool BNO055IMUDriver::is_imu_calibrated(void)
+bool CBNO055IMUDriver::is_imu_calibrated(void)
{
unsigned char calibration;
@@ -1081,7 +1085,7 @@ bool BNO055IMUDriver::is_imu_calibrated(void)
}
}
-BNO055IMUDriver::~BNO055IMUDriver()
+CBNO055IMUDriver::~CBNO055IMUDriver()
{
if(this->thread_server->get_thread_state(this->data_thread_id)==starting ||
this->thread_server->get_thread_state(this->data_thread_id)==active)
diff --git a/src/bno055_imu_driver.h b/src/bno055_imu_driver.h
index 235c5280b6974b1ec4c3647f92558cea38496805..3b81feaaed596e8a63e075076b9ceb4e588b4b31 100644
--- a/src/bno055_imu_driver.h
+++ b/src/bno055_imu_driver.h
@@ -150,7 +150,7 @@ typedef enum {temp_celcius,temp_farenheit} temp_units_t;
/* axis remapping definitions */
typedef enum {X_AXIS=0x00,Y_AXIS=0x01,Z_AXIS=0x02} axis_t;
-class BNO055IMUDriver
+class CBNO055IMUDriver
{
private:
std::string imu_name;
@@ -205,7 +205,7 @@ class BNO055IMUDriver
void get_sensor_units(void);
void get_sensor_configs(void);
public:
- BNO055IMUDriver(const std::string &name);
+ CBNO055IMUDriver(const std::string &name);
void open(const std::string &serial_dev);
void set_power_mode(power_mode_t power_mode);
power_mode_t get_power_mode(void);
@@ -242,7 +242,7 @@ class BNO055IMUDriver
bool is_magnetometer_calibrated(void);
bool is_gyroscope_calibrated(void);
bool is_imu_calibrated(void);
- ~BNO055IMUDriver();
+ ~CBNO055IMUDriver();
};
#endif
diff --git a/src/examples/bno055_imu_driver_test.cpp b/src/examples/bno055_imu_driver_test.cpp
index efbd98b5305e72719d206c9588e55f6b7af06a6c..7553f9a90cc9fe4dd82c4992fd19090cda2bdb36 100644
--- a/src/examples/bno055_imu_driver_test.cpp
+++ b/src/examples/bno055_imu_driver_test.cpp
@@ -6,28 +6,41 @@
int main(int argc, char *argv[])
{
try{
- bool accel_cal,mag_cal,gyro_cal,imu_cal;
+ bool accel_cal,mag_cal,gyro_cal;
CEventServer *event_server=CEventServer::instance();
std::list<std::string> events;
- BNO055IMUDriver imu("darwin_imu");
+ CBNO055IMUDriver imu("darwin_imu");
std::vector<double> accel,mag,gyro;
std::vector<double> quat,euler,linear_acc,gravity;
unsigned int i;
imu.open("/dev/ttyUSB1");
- imu.set_operation_mode(ndof_mode);
- do{
- accel_cal=imu.is_accelerometer_calibrated();
- std::cout << "Accelerometer calibrated: " << accel_cal << std::endl;
- mag_cal=imu.is_magnetometer_calibrated();
- std::cout << "Magnetometer calibrated: " << mag_cal << std::endl;
- gyro_cal=imu.is_gyroscope_calibrated();
- std::cout << "Gyroscope calibrated: " << gyro_cal << std::endl;
- imu_cal=imu.is_imu_calibrated();
- std::cout << "IMU calibrated: " << imu_cal << std::endl;
- sleep(1);
- }while(!accel_cal || !mag_cal || !gyro_cal || !imu_cal);
-
+ try{
+ imu.load_calibration("bno055.cal");
+ do{
+ imu.set_operation_mode(ndof_mode);
+ accel_cal=imu.is_accelerometer_calibrated();
+ std::cout << "Accelerometer calibrated: " << accel_cal << std::endl;
+ mag_cal=imu.is_magnetometer_calibrated();
+ std::cout << "Magnetometer calibrated: " << mag_cal << std::endl;
+ gyro_cal=imu.is_gyroscope_calibrated();
+ std::cout << "Gyroscope calibrated: " << gyro_cal << std::endl;
+ sleep(1);
+ }while(!accel_cal || !mag_cal || !gyro_cal);
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ do{
+ imu.set_operation_mode(ndof_mode);
+ accel_cal=imu.is_accelerometer_calibrated();
+ std::cout << "Accelerometer calibrated: " << accel_cal << std::endl;
+ mag_cal=imu.is_magnetometer_calibrated();
+ std::cout << "Magnetometer calibrated: " << mag_cal << std::endl;
+ gyro_cal=imu.is_gyroscope_calibrated();
+ std::cout << "Gyroscope calibrated: " << gyro_cal << std::endl;
+ sleep(1);
+ }while(!accel_cal || !mag_cal || !gyro_cal);
+ imu.save_calibration("bno055.cal");
+ }
events.push_back(imu.get_new_data_event_id());
for(i=0;i<1000;i++)
{