diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..40ebf8588f87960db1b3c5ccbab7c04fe7909d24 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +bin +lib +build diff --git a/Findbno055_imu_sim.cmake b/Findbno055_imu_sim.cmake deleted file mode 100644 index d535c23e11c081b8d798698db82b51dee5f9e9d0..0000000000000000000000000000000000000000 --- a/Findbno055_imu_sim.cmake +++ /dev/null @@ -1,21 +0,0 @@ -#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) - diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b7b96683553aa06999ff893d498c587e14bd3ad5..727e4a2c09b02aecf1cb774bb08d9940ab3ca22f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -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) diff --git a/src/bno055_imu_driver.cpp b/src/bno055_imu_driver.cpp index bcf989973dd2a01b5e7dce36743e2cc2544eee03..1475131140bcb7d48a87096b894f785a3a223ba8 100644 --- a/src/bno055_imu_driver.cpp +++ b/src/bno055_imu_driver.cpp @@ -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() diff --git a/src/bno055_imu_driver.h b/src/bno055_imu_driver.h index 2ddb42401e89e1734b818bbc00eb301ec56f3edf..f69a302197e3e4ca7b5940e4f15d55b9b73686de 100644 --- a/src/bno055_imu_driver.h +++ b/src/bno055_imu_driver.h @@ -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); diff --git a/src/bno055_imu_sim.cpp b/src/bno055_imu_sim.cpp deleted file mode 100644 index d4d0291f9ed77c8c1aee65162985da19c204a45d..0000000000000000000000000000000000000000 --- a/src/bno055_imu_sim.cpp +++ /dev/null @@ -1,370 +0,0 @@ -#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 - 0x00, // interrupt status - 0x00, // clock status - 0x00, // system status - 0x00, // system error code - 0x80, // units - 0xFF, // reserved - 0x1C, // operation mode - 0x00, // power mode - 0x00, // system trigger - 0x02, // temperature source - 0x00, // axis map config - 0x00, // axis map sign - 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, - 0x00,0x00,0x00,0x00,0x00,0x00,// acceleration offset - 0x00,0x00,0x00,0x00,0x00,0x00,// magnetometer offset - 0x00,0x00,0x00,0x00,0x00,0x00,// gyroscope offset - 0x00,0x00,// accelerometer radius - 0x00,0x00};// magnetometer radius - -unsigned char page1_data[]={0x00,0x00,0x00,0x00,0x00,0x00,0x00, //reserved - 0x01, // page ID - 0x0D, // accelerometer config - 0x6D, // magnetometer config - 0x38,0x00, // gyroscope config - 0x00, // accelerometer sleep config - 0x00, // gyroscope sleep config - 0x00, // reserved - 0x00, // interrupt mask - 0x00, // interrupt enable - 0x14, // accelerometer any motion threshold - 0x03, // accelerometer interrupt settings - 0x0F, // accelerometer High G duration - 0xC0, // accelerometer High G threshold - 0x0A, // accelerometer no/slow motion threshold - 0x0B, // accelerometer no/slow motion duration - 0x00, // gyroscope interrupt settings - 0x01, // gyroscope high rate thresold X - 0x19, // gyroscope high rate duration X - 0x01, // gyroscope high rate thresold Y - 0x19, // gyroscope high rate duration Y - 0x01, // gyroscope high rate thresold Z - 0x19, // gyroscope high rate duration Z - 0x04, // gyroscope any motion threshold - 0x0A, // gyroscope awake duration - 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, - 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, - 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, - 0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,0x0B,0x0C,0x0D,0x0E,0x0F};// BNO unique ID - -typedef enum {imu_header,imu_cmd,imu_address,imu_length,imu_data} receive_states_t; - -CBNO055IMUSim::CBNO055IMUSim(const std::string &name,const std::string &serial_dev) -{ - TRS232_config config; - - if(name.size()==0) - throw CBNO055IMUException(_HERE_,"Invalid IMU name"); - else - { - this->imu_name=name; - this->imu_port=NULL; - this->imu_device.clear(); - this->power_mode=normal_mode; - this->op_mode=config_mode; - this->current_page=0x00; - try{ - this->imu_port=new CRS232(this->imu_name+"_serial_port"); - this->imu_port->open((void *)&serial_dev); - config.baud=115200; - config.num_bits=8; - config.parity=none; - config.stop_bits=1; - this->imu_port->config(&config); - }catch(CException &s){ - if(this->imu_port!=NULL) - { - this->imu_port->close(); - delete this->imu_port; - this->imu_port=NULL; - } - throw; - } - /* initialize events */ - this->event_server=CEventServer::instance(); - this->finish_thread_event_id=this->imu_name+"_finish_thread_event"; - this->event_server->create_event(this->finish_thread_event_id); - /* initialize threads */ - this->thread_server=CThreadServer::instance(); - this->data_thread_id=this->imu_name+"_data_thread"; - this->thread_server->create_thread(this->data_thread_id); - this->thread_server->attach_thread(this->data_thread_id,this->data_thread,this); - this->thread_server->start_thread(this->data_thread_id); - } -} - -void *CBNO055IMUSim::data_thread(void *param) -{ - CBNO055IMUSim *imu=(CBNO055IMUSim *)param; - receive_states_t state=imu_header; - unsigned char address,length,op,*data,*serial_data,*cmd; - std::list<std::string> events; - unsigned int index,num,num_read=0,i,j; - bool end=false; - - events.push_back(imu->finish_thread_event_id); - events.push_back(imu->imu_port->get_rx_event_id()); - - while(!end) - { - index=imu->event_server->wait_first(events); - if(index==0) - end=true; - else if(index==1) - { - imu->data_access.enter(); - num=imu->imu_port->get_num_data(); - serial_data=new unsigned char[num]; - imu->imu_port->read(serial_data,num); - for(i=0;i<num;i++) - { - switch(state) - { - case imu_header: if(serial_data[i]==0xAA) - state=imu_cmd; - else - state=imu_header; - break; - case imu_cmd: state=imu_address; - op=serial_data[i]; - break; - case imu_address: state=imu_length; - address=serial_data[i]; - break; - case imu_length: length=serial_data[i]; - if(op==0x00) - { - state=imu_data; - data=new unsigned char[length]; - num_read=0; - } - else - { - state= imu_header; - // send the desired data - cmd=new unsigned char[2+length]; - cmd[0]=0xBB; - cmd[1]=length; - for(j=0;j<length;j++) - { - if(imu->current_page==0x00) - cmd[2+j]=page0_data[address+j]; - else - cmd[2+j]=page1_data[address+j]; - } - imu->imu_port->write(cmd,2+length); - delete[] cmd; - } - break; - case imu_data: data[num_read]=serial_data[i]; - num_read++; - if(num_read==length) - { - // process the write command - // send the response - cmd=new unsigned char[2]; - cmd[0]=0xBB; - cmd[1]=0x01; - imu->imu_port->write(cmd,2); - delete[] cmd; - delete[] data; - state=imu_header; - } - else - state=imu_data; - break; - } - } - delete[] serial_data; - imu->data_access.exit(); - } - } - - pthread_exit(NULL); -} - -void CBNO055IMUSim::process_write(unsigned char address, unsigned char length, unsigned char *data) -{ - unsigned int i=0; - - for(i=address;i<address+length;i++) - { - if(this->current_page==0x00) - { - page0_data[i]=data[i-address]; - switch(i) - { - case 0x07: this->current_page=data[i-address]; - break; - case 0x3B: if(data[i-address]&0x01) - this->accel_config.units=accel_gravity; - else - this->accel_config.units=accel_linear; - if(data[i-address]&0x02) - this->gyro_config.units=gyro_rad; - else - this->gyro_config.units=gyro_deg; - if(data[i-address]&0x04) - this->euler_units=euler_rad; - else - this->euler_units=euler_deg; - if(data[i-address]&0x10) - this->temp_units=temp_farenheit; - else - this->temp_units=temp_celcius; - break; - } - } - else - { - switch(i) - { - case 0x07: this->current_page=data[i-address]; - break; - } - } - } -} - -void CBNO055IMUSim::set_calibrated(void) -{ - page0_data[53]=0xFF; -} - -void CBNO055IMUSim::set_accel_data(double x, double y, double z) -{ - page0_data[8]=((int)(x*100.0))%256; - page0_data[9]=((int)(x*100.0))/256; - page0_data[10]=((int)(y*100.0))%256; - page0_data[11]=((int)(y*100.0))/256; - page0_data[12]=((int)(z*100.0))%256; - page0_data[13]=((int)(z*100.0))/256; -} - -void CBNO055IMUSim::set_gyro_data(double x, double y, double z) -{ - if(this->gyro_config.units==gyro_deg) - { - page0_data[20]=((int)(x*16.0))%256; - page0_data[21]=((int)(x*16.0))/256; - page0_data[22]=((int)(y*16.0))%256; - page0_data[23]=((int)(y*16.0))/256; - page0_data[24]=((int)(z*16.0))%256; - page0_data[25]=((int)(z*16.0))/256; - } - else - { - page0_data[20]=((int)(x*900.0))%256; - page0_data[21]=((int)(x*900.0))/256; - page0_data[22]=((int)(y*900.0))%256; - page0_data[23]=((int)(y*900.0))/256; - page0_data[24]=((int)(z*900.0))%256; - page0_data[25]=((int)(z*900.0))/256; - } -} - -void CBNO055IMUSim::set_mag_data(double x, double y, double z) -{ - page0_data[14]=((int)(x*16.0))%256; - page0_data[15]=((int)(x*16.0))/256; - page0_data[16]=((int)(y*16.0))%256; - page0_data[17]=((int)(y*16.0))/256; - page0_data[18]=((int)(z*16.0))%256; - page0_data[19]=((int)(z*16.0))/256; -} - -void CBNO055IMUSim::set_quaternion_data(double x, double y, double z,double w) -{ - page0_data[32]=((int)(w*16384.0))%256; - page0_data[33]=((int)(w*16384.0))/256; - page0_data[34]=((int)(x*16384.0))%256; - page0_data[35]=((int)(x*16384.0))/256; - page0_data[36]=((int)(y*16384.0))%256; - page0_data[37]=((int)(y*16384.0))/256; - page0_data[38]=((int)(z*16384.0))%256; - page0_data[39]=((int)(z*16384.0))/256; -} - -void CBNO055IMUSim::set_euler_angles_data(double yaw, double roll, double pitch) -{ - if(this->euler_units==euler_deg) - { - page0_data[26]=((int)(yaw*16.0))%256; - page0_data[27]=((int)(yaw*16.0))/256; - page0_data[28]=((int)(roll*16.0))%256; - page0_data[29]=((int)(roll*16.0))/256; - page0_data[30]=((int)(pitch*16.0))%256; - page0_data[31]=((int)(pitch*16.0))/256; - } - else - { - page0_data[26]=((int)(yaw*900.0))%256; - page0_data[27]=((int)(yaw*900.0))/256; - page0_data[28]=((int)(roll*900.0))%256; - page0_data[29]=((int)(roll*900.0))/256; - page0_data[30]=((int)(pitch*900.0))%256; - page0_data[31]=((int)(pitch*900.0))/256; - } -} - -void CBNO055IMUSim::set_linear_accel_data(double x, double y, double z) -{ - page0_data[40]=((int)(x*100.0))%256; - page0_data[41]=((int)(x*100.0))/256; - page0_data[42]=((int)(y*100.0))%256; - page0_data[43]=((int)(y*100.0))/256; - page0_data[44]=((int)(z*100.0))%256; - page0_data[45]=((int)(z*100.0))/256; -} - -void CBNO055IMUSim::set_gravity_data(double x, double y, double z) -{ - page0_data[46]=((int)(x*100.0))%256; - page0_data[47]=((int)(x*100.0))/256; - page0_data[48]=((int)(y*100.0))%256; - page0_data[49]=((int)(y*100.0))/256; - page0_data[50]=((int)(z*100.0))%256; - page0_data[51]=((int)(z*100.0))/256; -} - -CBNO055IMUSim::~CBNO055IMUSim() -{ - 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->thread_server->detach_thread(this->data_thread_id); - this->thread_server->delete_thread(this->data_thread_id); - this->data_thread_id=""; - this->event_server->delete_event(this->finish_thread_event_id); - this->finish_thread_event_id=""; - if(this->imu_port!=NULL) - { - this->imu_port->close(); - delete this->imu_port; - this->imu_port=NULL; - } -} - diff --git a/src/bno055_imu_sim.h b/src/bno055_imu_sim.h deleted file mode 100644 index 0628c3265c87957a524b5f4e9b021b2aa7416219..0000000000000000000000000000000000000000 --- a/src/bno055_imu_sim.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef _BNO055_IMU_SIM_H -#define _BNO055_IMU_SIM_H - -#include "rs232.h" -#include "mutex.h" -#include "threadserver.h" -#include "bno055_common.h" -#include <vector> - -class CBNO055IMUSim -{ - private: - std::string imu_name; - CRS232 *imu_port; - std::string imu_device; - CMutex imu_access; - power_mode_t power_mode; - op_mode_t op_mode; - unsigned char current_page; - /* thread attributes */ - CThreadServer *thread_server; - std::string data_thread_id; - /* event attributes */ - CEventServer *event_server; - std::string finish_thread_event_id; - /* sensor data */ - CMutex data_access; - /* accelerometer data */ - TBNO055AccelConfig accel_config; - /* magnetometer data */ - TBNO055MagConfig mag_config; - /* gyroscope data */ - TBNO055GyroConfig gyro_config; - /* temperature data */ - temp_units_t temp_units; - /* fusion data */ - euler_units_t euler_units; - protected: - static void *data_thread(void *param); - void process_write(unsigned char address, unsigned char length, unsigned char *data); - public: - CBNO055IMUSim(const std::string &name,const std::string &serial_dev); - void set_calibrated(void); - void set_accel_data(double x, double y, double z); - void set_gyro_data(double x, double y, double z); - void set_mag_data(double x, double y, double z); - void set_quaternion_data(double x, double y, double z,double w); - void set_euler_angles_data(double yaw, double roll, double pitch); - void set_linear_accel_data(double x, double y, double z); - void set_gravity_data(double x, double y, double z); - ~CBNO055IMUSim(); -}; - -#endif diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 586bfd62897e2bfbc341b5ce3d6b2b20c68da8e1..fd72ae5a9b0a36a24c83a0ca340577a11a071117 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -2,8 +2,3 @@ ADD_EXECUTABLE(bno055_imu_driver_test bno055_imu_driver_test.cpp) # link necessary libraries TARGET_LINK_LIBRARIES(bno055_imu_driver_test bno055_imu_driver) - -# create an example application -ADD_EXECUTABLE(bno055_imu_sim_test bno055_imu_sim_test.cpp) -# link necessary libraries -TARGET_LINK_LIBRARIES(bno055_imu_sim_test bno055_imu_sim) diff --git a/src/examples/bno055_imu_driver_test.cpp b/src/examples/bno055_imu_driver_test.cpp index d5722fe25b428aa81feaefa8f232943e817432f0..55498fcd4009fffb7b9e394d201c9f0e561e43f2 100644 --- a/src/examples/bno055_imu_driver_test.cpp +++ b/src/examples/bno055_imu_driver_test.cpp @@ -14,34 +14,21 @@ int main(int argc, char *argv[]) std::vector<double> quat,euler,linear_acc,gravity; unsigned int i; - imu.open("/dev/pts/23"); + imu.open("/tmp/darwin1_imu_uart"); 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"); } + imu.set_operation_mode(acc_mag_gyro_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; events.push_back(imu.get_new_data_event_id()); + for(i=0;i<1000;i++) { event_server->wait_all(events,1000); @@ -53,12 +40,12 @@ int main(int argc, char *argv[]) std::cout << "Linear acceleration: X: " << linear_acc[0] << ", Y: " << linear_acc[1] << ", Z: " << linear_acc[2] << std::endl; gravity=imu.get_gravity(); std::cout << "Gravity: X: " << gravity[0] << ", Y: " << gravity[1] << ", Z: " << gravity[2] << std::endl; -/* accel=imu.get_raw_accelerometer(); + accel=imu.get_raw_accelerometer(); std::cout << "Accelerometer: X: " << accel[0] << ", Y: " << accel[1] << ", Z: " << accel[2] << std::endl; mag=imu.get_raw_magnetometer(); std::cout << "Magnetometer: X: " << mag[0] << ", Y: " << mag[1] << ", Z: " << mag[2] << std::endl; gyro=imu.get_raw_gyroscope(); - std::cout << "Gyroscope: X: " << gyro[0] << ", Y: " << gyro[1] << ", Z: " << gyro[2] << std::endl;*/ + std::cout << "Gyroscope: X: " << gyro[0] << ", Y: " << gyro[1] << ", Z: " << gyro[2] << std::endl; } }catch(CException &e){ diff --git a/src/examples/bno055_imu_sim_test.cpp b/src/examples/bno055_imu_sim_test.cpp deleted file mode 100644 index 1c783f49ffd44956620e0b20de567b1ea486fd85..0000000000000000000000000000000000000000 --- a/src/examples/bno055_imu_sim_test.cpp +++ /dev/null @@ -1,19 +0,0 @@ -#include "bno055_imu_sim.h" -#include "bno055_imu_exceptions.h" -#include <unistd.h> -#include <iostream> - -int main(int argc, char *argv[]) -{ - try{ - CBNO055IMUSim imu("darwin_imu","/dev/pts/23"); - - sleep(3); - imu.set_calibrated(); - - while(1); - - }catch(CException &e){ - std::cout << e.what() << std::endl; - } -}