Skip to content
GitLab
Commit 207d84c6 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Moved the simulator part to a separate repository.

parent 54579c2b
Hide whitespace changes
Inline Side-by-side
Findbno055_imu_sim.cmake deleted 100644 → 0
View file @ 54579c2b
#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)
src/CMakeLists.txt
View file @ 207d84c6
......@@ -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)
src/bno055_imu_sim.cpp deleted 100644 → 0
View file @ 54579c2b
#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;
}
}
src/bno055_imu_sim.h deleted 100644 → 0
View file @ 54579c2b
#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
src/examples/CMakeLists.txt
View file @ 207d84c6
......@@ -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)
src/examples/bno055_imu_driver_test.cpp
View file @ 207d84c6
......@@ -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){
......
src/examples/bno055_imu_sim_test.cpp deleted 100644 → 0
View file @ 54579c2b
#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;
}
}
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