Commit cb443ded authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files
parents aa74d37d 5e42e3e4
bin
lib
build
#edit the following line to add the librarie's header files
FIND_PATH(bno055_imu_sim_INCLUDE_DIR bno055_imu_sim.h /usr/include/iridrivers /usr/local/include/iridrivers)
FIND_LIBRARY(bno055_imu_sim_LIBRARY
NAMES bno055_imu_sim
PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers)
IF (bno055_imu_sim_INCLUDE_DIR AND bno055_imu_sim_LIBRARY)
SET(bno055_imu_sim_FOUND TRUE)
ENDIF (bno055_imu_sim_INCLUDE_DIR AND bno055_imu_sim_LIBRARY)
IF (bno055_imu_sim_FOUND)
IF (NOT bno055_imu_sim_FIND_QUIETLY)
MESSAGE(STATUS "Found bno055_imu_sim: ${bno055_imu_sim_LIBRARY}")
ENDIF (NOT bno055_imu_sim_FIND_QUIETLY)
ELSE (bno055_imu_sim_FOUND)
IF (bno055_imu_sim_FIND_REQUIRED)
MESSAGE(FATAL_ERROR "Could not find bno055_imu_sim")
ENDIF (bno055_imu_sim_FIND_REQUIRED)
ENDIF (bno055_imu_sim_FOUND)
......@@ -2,10 +2,6 @@
SET(sources bno055_imu_driver.cpp bno055_imu_exceptions.cpp)
# application header files
SET(headers bno055_imu_driver.h bno055_imu_exceptions.h bno055_common.h)
# simulator source files
SET(sources_sim bno055_imu_sim.cpp bno055_imu_exceptions.cpp)
# application header files
SET(headers_sim bno055_imu_sim.h bno055_imu_exceptions.h bno055_common.h)
# locate the necessary dependencies
FIND_PACKAGE(iriutils REQUIRED)
FIND_PACKAGE(comm REQUIRED)
......@@ -19,23 +15,11 @@ ADD_LIBRARY(bno055_imu_driver SHARED ${sources})
TARGET_LINK_LIBRARIES(bno055_imu_driver ${iriutils_LIBRARY})
TARGET_LINK_LIBRARIES(bno055_imu_driver ${comm_LIBRARY})
# create the shared library
ADD_LIBRARY(bno055_imu_sim SHARED ${sources_sim})
# link necessary libraries
TARGET_LINK_LIBRARIES(bno055_imu_sim ${iriutils_LIBRARY})
TARGET_LINK_LIBRARIES(bno055_imu_sim ${comm_LIBRARY})
INSTALL(TARGETS bno055_imu_driver
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iridrivers
ARCHIVE DESTINATION lib/iridrivers)
INSTALL(FILES ${headers} DESTINATION include/iridrivers)
INSTALL(TARGETS bno055_imu_sim
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iridrivers
ARCHIVE DESTINATION lib/iridrivers)
INSTALL(FILES ${headers_sim} DESTINATION include/iridrivers)
INSTALL(FILES ../Findbno055_imu_driver.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
INSTALL(FILES ../Findbno055_imu_sim.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
ADD_SUBDIRECTORY(examples)
......@@ -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;
}
}
......@@ -84,10 +89,20 @@ void CBNO055IMUDriver::convert_gyro_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;
this->quaternion_data[1]=((signed short int)(data[4]+(data[5]<<8)))/16384.0;
this->quaternion_data[2]=((signed short int)(data[6]+(data[7]<<8)))/16384.0;
if(this->op_mode>=imu_mode)
{
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;
this->quaternion_data[1]=((signed short int)(data[4]+(data[5]<<8)))/16384.0;
this->quaternion_data[2]=((signed short int)(data[6]+(data[7]<<8)))/16384.0;
}
else
{
this->quaternion_data[3]=1.0;
this->quaternion_data[0]=0.0;
this->quaternion_data[1]=0.0;
this->quaternion_data[2]=0.0;
}
}
void CBNO055IMUDriver::convert_euler_angles_data(unsigned char *data)
......@@ -132,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)
......@@ -149,45 +192,22 @@ void CBNO055IMUDriver::change_register_page(unsigned char page_id)
void CBNO055IMUDriver::process_data(unsigned char *data)
{
this->data_access.enter();
switch(this->op_mode)
{
case config_mode: break;
case acc_only_mode: this->convert_accel_data(data);
break;
case mag_only_mode: this->convert_mag_data(data);
break;
case gyro_only_mode: this->convert_gyro_data(data);
break;
case acc_mag_mode: this->convert_accel_data(data);
this->convert_mag_data(&data[6]);
break;
case acc_gyro_mode: this->convert_accel_data(data);
this->convert_gyro_data(&data[6]);
break;
case mag_gyro_mode: this->convert_mag_data(data);
this->convert_gyro_data(&data[6]);
break;
case acc_mag_gyro_mode: this->convert_accel_data(data);
this->convert_mag_data(&data[6]);
this->convert_gyro_data(&data[12]);
break;
case imu_mode:
case compass_mode:
case m4g_mode:
case ndof_off_mode:
case ndof_mode: this->convert_euler_angles_data(data);
this->convert_quaternion_data(&data[6]);
this->convert_linear_accel_data(&data[14]);
this->convert_gravity_data(&data[20]);
break;
}
this->convert_accel_data(data);
this->convert_mag_data(&data[6]);
this->convert_gyro_data(&data[12]);
this->convert_euler_angles_data(&data[18]);
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,length,*data;
unsigned char address=0x08,length=46,data[46];
bool end=false;
while(!end)
......@@ -196,49 +216,7 @@ void *CBNO055IMUDriver::data_thread(void *param)
end=true;
else
{
switch(imu->op_mode)
{
case config_mode: address=0x00;
length=0x00;
break;
case acc_only_mode: address=0x08;
length=6;
data=new unsigned char[length];
break;
case mag_only_mode: address=0x0E;
length=6;
data=new unsigned char[length];
break;
case gyro_only_mode: address=0x14;
length=6;
data=new unsigned char[length];
break;
case acc_mag_mode: address=0x08;
length=12;
data=new unsigned char[length];
break;
case acc_gyro_mode: address=0x08;
length=18;
data=new unsigned char[length];
break;
case mag_gyro_mode: address=0x0E;
length=12;
data=new unsigned char[length];
break;
case acc_mag_gyro_mode: address=0x08;
length=18;
data=new unsigned char[length];
break;
case imu_mode:
case compass_mode:
case m4g_mode:
case ndof_off_mode:
case ndof_mode: address=0x1A;
length=26;
data=new unsigned char[length];
break;
}
if(length>0)
if(imu->op_mode!=config_mode)
{
try{
imu->imu_access.enter();
......@@ -246,11 +224,8 @@ void *CBNO055IMUDriver::data_thread(void *param)
imu->read_registers(address,length,data);
imu->imu_access.exit();
imu->process_data(data);
delete[] data;
if(!imu->event_server->event_is_set(imu->new_data_event_id))
{
imu->event_server->set_event(imu->new_data_event_id);
}
}catch(CException &e){
imu->imu_access.exit();
std::cout << e.what() << std::endl;
......@@ -464,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");
......@@ -477,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();
......@@ -533,83 +511,38 @@ void CBNO055IMUDriver::set_operation_mode(op_mode_t op_mode)
{
if(this->op_mode!=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(op_mode==config_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);
this->event_server->reset_event(this->finish_thread_event_id);
}
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;
}
else
{
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_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;
}
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->thread_server->get_thread_state(this->data_thread_id)==attached)
this->thread_server->start_thread(this->data_thread_id);
}
......@@ -893,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 */
......@@ -961,28 +886,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)
......@@ -1011,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);
......
#include "bno055_imu_sim.h"
#include "bno055_imu_exceptions.h"
#include "eventexceptions.h"
#include <iostream>
#include <fstream>
#include <math.h>
unsigned char page0_data[]={0xA0,0xFB,0x32,0x0F,// chip ID's
0x08,0x03,0x00,// version ID's
0x00,//page 0 ID
0x00,0x00,0x00,0x00,0x00,0x00,// acceleration data
0x00,0x00,0x00,0x00,0x00,0x00,// magnetometer data
0x00,0x00,0x00,0x00,0x00,0x00,// gyroscope data
0x00,0x00,0x00,0x00,0x00,0x00,// euler data
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,// quaternion data
0x00,0x00,0x00,0x00,0x00,0x00,// linear acceleration data
0x00,0x00,0x00,0x00,0x00,0x00,// gravity data
0x00, // temperature
0x00, // calibration status
0x0F, // ST result