Commit 54579c2b authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Read all the on-line data from the main thread (added calibration and temperature data).

The corresponding get functions return the internal data value.
parent 962273b1
......@@ -23,6 +23,10 @@ CBNO055IMUDriver::CBNO055IMUDriver(const std::string &name)
this->info.soft_ver=0xFFFF;
this->info.boot_ver=0xFF;
this->current_page=0x00;
this->accel_calibrated=false;
this->mag_calibrated=false;
this->gyro_calibrated=false;
this->system_calibrated=false;
/* initialize events */
this->event_server=CEventServer::instance();
this->finish_thread_event_id=this->imu_name+"_finish_thread_event";
......@@ -43,6 +47,7 @@ CBNO055IMUDriver::CBNO055IMUDriver(const std::string &name)
this->euler_angles_data.resize(3,0.0);
this->linear_accel_data.resize(3,0.0);
this->gravity_data.resize(3,0.0);
this->temperature=0.0;
}
}
......@@ -142,6 +147,34 @@ void CBNO055IMUDriver::convert_gravity_data(unsigned char *data)
}
}
void CBNO055IMUDriver::convert_temperature_data(unsigned char *data)
{
if(this->temp_units==temp_celcius)
this->temperature=(double)data[0];
else
this->temperature=(double)(data[0]*2.0);
}
void CBNO055IMUDriver::convert_calibration_data(unsigned char *data)
{
if((data[0]&0x03)==0x03)
this->mag_calibrated=true;
else
this->mag_calibrated=false;
if((data[0]&0x0C)==0x0C)
this->accel_calibrated=true;
else
this->accel_calibrated=false;
if((data[0]&0x30)==0x30)
this->gyro_calibrated=true;
else
this->gyro_calibrated=false;
if((data[0]&0xC0)==0xC0)
this->system_calibrated=true;
else
this->system_calibrated=false;
}
void CBNO055IMUDriver::change_register_page(unsigned char page_id)
{
if(page_id!=0x01 && page_id!=0x00)
......@@ -166,13 +199,15 @@ void CBNO055IMUDriver::process_data(unsigned char *data)
this->convert_quaternion_data(&data[24]);
this->convert_linear_accel_data(&data[32]);
this->convert_gravity_data(&data[38]);
this->convert_temperature_data(&data[44]);
this->convert_calibration_data(&data[45]);
this->data_access.exit();
}
void *CBNO055IMUDriver::data_thread(void *param)
{
CBNO055IMUDriver *imu=(CBNO055IMUDriver *)param;
unsigned char address=0x08,length=44,data[44];
unsigned char address=0x08,length=46,data[46];
bool end=false;
while(!end)
......@@ -404,6 +439,7 @@ void CBNO055IMUDriver::get_sensor_configs(void)
void CBNO055IMUDriver::open(const std::string &serial_dev)
{
TRS232_config config;
unsigned char cal_data;
try{
this->imu_port=new CRS232(this->imu_name+"_serial_port");
......@@ -417,6 +453,8 @@ void CBNO055IMUDriver::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->read_registers(0x35,1,&cal_data);
this->convert_calibration_data(&cal_data);
this->op_mode=this->get_operation_mode();
this->set_operation_mode(config_mode);
this->power_mode=this->get_power_mode();
......@@ -788,21 +826,13 @@ void CBNO055IMUDriver::get_gyro_config(unsigned char *range,unsigned char *bandw
/* temperature functions */
double CBNO055IMUDriver::get_temperature(void)
{
unsigned char temperature;
double data;
try{
this->imu_access.enter();
this->change_register_page(0x00);
this->read_registers(0x34,1,&temperature);
this->imu_access.exit();
if(this->temp_units==temp_celcius)
return (double)temperature;
else
return (double)(temperature*=2.0);
}catch(CException &e){
this->imu_access.exit();
throw;
}
this->data_access.enter();
data=this->temperature;
this->data_access.exit();
return data;
}
/* fused data functions */
......@@ -906,78 +936,46 @@ void CBNO055IMUDriver::save_calibration(const std::string &filename)
bool CBNO055IMUDriver::is_accelerometer_calibrated(void)
{
unsigned char calibration;
bool data;
try{
this->imu_access.enter();
this->change_register_page(0x00);
this->read_registers(0x35,1,&calibration);
this->imu_access.exit();
if((calibration&0x0C)==0x0C)
return true;
else
return false;
}catch(CException &e){
this->imu_access.exit();
throw;
}
this->data_access.enter();
data=this->accel_calibrated;
this->data_access.exit();
return data;
}
bool CBNO055IMUDriver::is_magnetometer_calibrated(void)
{
unsigned char calibration;
bool data;
try{
this->imu_access.enter();
this->change_register_page(0x00);
this->read_registers(0x35,1,&calibration);
this->imu_access.exit();
if((calibration&0x03)==0x03)
return true;
else
return false;
}catch(CException &e){
this->imu_access.exit();
throw;
}
this->data_access.enter();
data=this->mag_calibrated;
this->data_access.exit();
return data;
}
bool CBNO055IMUDriver::is_gyroscope_calibrated(void)
{
unsigned char calibration;
bool data;
try{
this->imu_access.enter();
this->change_register_page(0x00);
this->read_registers(0x35,1,&calibration);
this->imu_access.exit();
if((calibration&0x30)==0x30)
return true;
else
return false;
}catch(CException &e){
this->imu_access.exit();
throw;
}
this->data_access.enter();
data=this->gyro_calibrated;
this->data_access.exit();
return data;
}
bool CBNO055IMUDriver::is_imu_calibrated(void)
{
unsigned char calibration;
bool data;
try{
this->imu_access.enter();
this->change_register_page(0x00);
this->read_registers(0x35,1,&calibration);
this->imu_access.exit();
if((calibration&0xC0)==0xC0)
return true;
else
return false;
}catch(CException &e){
this->imu_access.exit();
throw;
}
this->data_access.enter();
data=this->system_calibrated;
this->data_access.exit();
return data;
}
CBNO055IMUDriver::~CBNO055IMUDriver()
......
......@@ -28,6 +28,11 @@ class CBNO055IMUDriver
std::string new_data_event_id;
/* sensor data */
CMutex data_access;
/* calibration data */
bool accel_calibrated;
bool mag_calibrated;
bool gyro_calibrated;
bool system_calibrated;
/* accelerometer data */
std::vector<double> raw_accel_data;
TBNO055AccelConfig accel_config;
......@@ -39,6 +44,7 @@ class CBNO055IMUDriver
TBNO055GyroConfig gyro_config;
/* temperature data */
temp_units_t temp_units;
double temperature;
/* fusion data */
std::vector<double> quaternion_data;
std::vector<double> euler_angles_data;
......@@ -53,6 +59,8 @@ class CBNO055IMUDriver
void convert_euler_angles_data(unsigned char *data);
void convert_linear_accel_data(unsigned char *data);
void convert_gravity_data(unsigned char *data);
void convert_temperature_data(unsigned char *data);
void convert_calibration_data(unsigned char *data);
void change_register_page(unsigned char page_id);
void process_data(unsigned char *data);
static void *data_thread(void *param);
......
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