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_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 c003e48119492c8168943da635759a44b6960982..55498fcd4009fffb7b9e394d201c9f0e561e43f2 100644 --- a/src/examples/bno055_imu_driver_test.cpp +++ b/src/examples/bno055_imu_driver_test.cpp @@ -14,7 +14,7 @@ int main(int argc, char *argv[]) std::vector<double> quat,euler,linear_acc,gravity; unsigned int i; - imu.open("/dev/ttyUSB2"); + imu.open("/tmp/darwin1_imu_uart"); try{ imu.load_calibration("bno055.cal"); }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; - } -}