Skip to content
Snippets Groups Projects
Commit 8b7a66bb authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a new library with an implementation of a simulated BNO055 IMU (for the Gazebo simulator)

parent 69b75e83
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 637 additions and 146 deletions
Findbno055_imu_sim.cmake 0 → 100644
+ 21
− 0
View file @ 8b7a66bb
#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
+ 18
− 1
View file @ 8b7a66bb
# driver source files # driver source files
SET(sources bno055_imu_driver.cpp bno055_imu_exceptions.cpp) SET(sources bno055_imu_driver.cpp bno055_imu_exceptions.cpp)
# application header files # application header files
SET(headers bno055_imu_driver.h bno055_imu_exceptions.h) 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 # locate the necessary dependencies
FIND_PACKAGE(iriutils REQUIRED) FIND_PACKAGE(iriutils REQUIRED)
FIND_PACKAGE(comm REQUIRED) FIND_PACKAGE(comm REQUIRED)
...@@ -15,10 +19,23 @@ ADD_LIBRARY(bno055_imu_driver SHARED ${sources}) ...@@ -15,10 +19,23 @@ ADD_LIBRARY(bno055_imu_driver SHARED ${sources})
TARGET_LINK_LIBRARIES(bno055_imu_driver ${iriutils_LIBRARY}) TARGET_LINK_LIBRARIES(bno055_imu_driver ${iriutils_LIBRARY})
TARGET_LINK_LIBRARIES(bno055_imu_driver ${comm_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 INSTALL(TARGETS bno055_imu_driver
RUNTIME DESTINATION bin RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iridrivers LIBRARY DESTINATION lib/iridrivers
ARCHIVE DESTINATION lib/iridrivers) ARCHIVE DESTINATION lib/iridrivers)
INSTALL(FILES ${headers} DESTINATION include/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_driver.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
INSTALL(FILES ../Findbno055_imu_sim.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
ADD_SUBDIRECTORY(examples) ADD_SUBDIRECTORY(examples)
src/bno055_common.h 0 → 100644
+ 148
− 0
View file @ 8b7a66bb
#ifndef BNO055_COMMON_H
#define BNO055_COMMON_H
typedef enum {normal_mode=0x00,
low_power_mode=0x01,
suspend_mode=0x02} power_mode_t;
typedef enum {config_mode=0x00,
acc_only_mode=0x01,
mag_only_mode=0x02,
gyro_only_mode=0x03,
acc_mag_mode=0x04,
acc_gyro_mode=0x05,
mag_gyro_mode=0x06,
acc_mag_gyro_mode=0x07,
imu_mode=0x08,
compass_mode=0x09,
m4g_mode=0x0A,
ndof_off_mode=0x0B,
ndof_mode=0x0C} op_mode_t;
typedef struct
{
unsigned char chip_id;
unsigned char acc_id;
unsigned char mag_id;
unsigned char gyr_id;
unsigned short int soft_ver;
unsigned char boot_ver;
}TBNO055IMUInfo;
/* accelerometer definitions */
/* operation mode */
#define ACCEL_MODE_MASK 0xE0
#define ACCEL_NORMAL_MODE 0x00
#define ACCEL_SUSPEND_MODE 0x20
#define ACCEL_LOW_POWER1_MODE 0x40
#define ACCEL_STANDBY_MODE 0x60
#define ACCEL_LOW_POWER2_MODE 0x80
#define ACCEL_DEEP_SUSPEND_MODE 0xA0
/* range */
#define ACCEL_RANGE_MASK 0x03
#define ACCEL_2G_RANGE 0x00
#define ACCEL_4G_RANGE 0x01
#define ACCEL_8G_RANGE 0x02
#define ACCEL_16G_RANGE 0x03
/* bandwidth */
#define ACCEL_BW_MASK 0x1C
#define ACCEL_7_81_BW 0x00
#define ACCEL_15_63_BW 0x04
#define ACCEL_31_25_BW 0x08
#define ACCEL_62_5_BW 0x0C
#define ACCEL_125_BW 0x10
#define ACCEL_250_BW 0x14
#define ACCEL_500_BW 0x18
#define ACCEL_1000_BW 0x1C
typedef enum {accel_linear,accel_gravity} accel_units_t;
typedef struct
{
unsigned char conf_reg;
accel_units_t units;
}TBNO055AccelConfig;
/* gyroscope definitions */
/* operation mode */
#define GYRO_MODE_MASK 0x07
#define GYRO_NORMAL_MODE 0x00
#define GYRO_FAST_PWR_UP_MODE 0x01
#define GYRO_DEEP_SUSPEND_MODE 0x02
#define GYRO_SUSPEND_MODE 0x03
#define GYRO_ADV_POWER_SAVE_MODE 0x04
/* range */
#define GYRO_RANGE_MASK 0x07
#define GYRO_2000_RANGE 0x00
#define GYRO_1000_RANGE 0x01
#define GYRO_500_RANGE 0x02
#define GYRO_250_RANGE 0x03
#define GYRO_125_RANGE 0x04
/* bandwidth */
#define GYRO_BW_MASK 0x38
#define GYRO_523_BW 0x00
#define GYRO_230_BW 0x08
#define GYRO_116_BW 0x10
#define GYRO_47_BW 0x18
#define GYRO_23_BW 0x20
#define GYRO_12_BW 0x28
#define GYRO_64_BW 0x30
#define GYRO_32_BW 0x38
typedef enum {gyro_deg,gyro_rad} gyro_units_t;
typedef enum {euler_deg,euler_rad} euler_units_t;
typedef struct
{
unsigned char conf_reg1;
unsigned char conf_reg2;
gyro_units_t units;
}TBNO055GyroConfig;
/* magnetometer definitions */
/* operation mode */
#define MAG_OP_MODE_MASK 0x18
#define MAG_LOW_POWER_MODE 0x00
#define MAG_REGULAR_MODE 0x08
#define MAG_EN_REGULAR_MODE 0x10
#define MAG_HICH_ACC_MODE 0x18
/* power mode */
#define MAG_POWER_MODE_MASK 0x60
#define MAG_NORMAL_MODE 0x00
#define MAG_SLEEP_MODE 0x20
#define MAG_SUSPEND_MODE 0x40
#define MAG_FORCE_MODE 0x60
/* rate */
#define MAG_RATE_MASK 0x07
#define MAG_2_RATE 0x00
#define MAG_6_RATE 0x01
#define MAG_8_RATE 0x02
#define MAG_10_RATE 0x03
#define MAG_15_RATE 0x04
#define MAG_20_RATE 0x05
#define MAG_25_RATE 0x06
#define MAG_30_RATE 0x07
typedef struct
{
unsigned char conf_reg;
}TBNO055MagConfig;
#define ACCEL_UNITS_MASK 0x01
#define GYRO_UNITS_MASK 0x02
#define EULER_UNITS_MASK 0x04
#define TEMP_UNITS_MASK 0x10
/* temperature definitions */
typedef enum {temp_celcius,temp_farenheit} temp_units_t;
/* axis remapping definitions */
typedef enum {X_AXIS=0x00,Y_AXIS=0x01,Z_AXIS=0x02} axis_t;
#endif
src/bno055_imu_driver.h
+ 1
− 144
View file @ 8b7a66bb
...@@ -4,152 +4,9 @@ ...@@ -4,152 +4,9 @@
#include "rs232.h" #include "rs232.h"
#include "mutex.h" #include "mutex.h"
#include "threadserver.h" #include "threadserver.h"
#include "bno055_common.h"
#include <vector> #include <vector>
typedef enum {normal_mode=0x00,
low_power_mode=0x01,
suspend_mode=0x02} power_mode_t;
typedef enum {config_mode=0x00,
acc_only_mode=0x01,
mag_only_mode=0x02,
gyro_only_mode=0x03,
acc_mag_mode=0x04,
acc_gyro_mode=0x05,
mag_gyro_mode=0x06,
acc_mag_gyro_mode=0x07,
imu_mode=0x08,
compass_mode=0x09,
m4g_mode=0x0A,
ndof_off_mode=0x0B,
ndof_mode=0x0C} op_mode_t;
typedef struct
{
unsigned char chip_id;
unsigned char acc_id;
unsigned char mag_id;
unsigned char gyr_id;
unsigned short int soft_ver;
unsigned char boot_ver;
}TBNO055IMUInfo;
/* accelerometer definitions */
/* operation mode */
#define ACCEL_MODE_MASK 0xE0
#define ACCEL_NORMAL_MODE 0x00
#define ACCEL_SUSPEND_MODE 0x20
#define ACCEL_LOW_POWER1_MODE 0x40
#define ACCEL_STANDBY_MODE 0x60
#define ACCEL_LOW_POWER2_MODE 0x80
#define ACCEL_DEEP_SUSPEND_MODE 0xA0
/* range */
#define ACCEL_RANGE_MASK 0x03
#define ACCEL_2G_RANGE 0x00
#define ACCEL_4G_RANGE 0x01
#define ACCEL_8G_RANGE 0x02
#define ACCEL_16G_RANGE 0x03
/* bandwidth */
#define ACCEL_BW_MASK 0x1C
#define ACCEL_7_81_BW 0x00
#define ACCEL_15_63_BW 0x04
#define ACCEL_31_25_BW 0x08
#define ACCEL_62_5_BW 0x0C
#define ACCEL_125_BW 0x10
#define ACCEL_250_BW 0x14
#define ACCEL_500_BW 0x18
#define ACCEL_1000_BW 0x1C
typedef enum {accel_linear,accel_gravity} accel_units_t;
typedef struct
{
unsigned char conf_reg;
accel_units_t units;
}TBNO055AccelConfig;
/* gyroscope definitions */
/* operation mode */
#define GYRO_MODE_MASK 0x07
#define GYRO_NORMAL_MODE 0x00
#define GYRO_FAST_PWR_UP_MODE 0x01
#define GYRO_DEEP_SUSPEND_MODE 0x02
#define GYRO_SUSPEND_MODE 0x03
#define GYRO_ADV_POWER_SAVE_MODE 0x04
/* range */
#define GYRO_RANGE_MASK 0x07
#define GYRO_2000_RANGE 0x00
#define GYRO_1000_RANGE 0x01
#define GYRO_500_RANGE 0x02
#define GYRO_250_RANGE 0x03
#define GYRO_125_RANGE 0x04
/* bandwidth */
#define GYRO_BW_MASK 0x38
#define GYRO_523_BW 0x00
#define GYRO_230_BW 0x08
#define GYRO_116_BW 0x10
#define GYRO_47_BW 0x18
#define GYRO_23_BW 0x20
#define GYRO_12_BW 0x28
#define GYRO_64_BW 0x30
#define GYRO_32_BW 0x38
typedef enum {gyro_deg,gyro_rad} gyro_units_t;
typedef enum {euler_deg,euler_rad} euler_units_t;
typedef struct
{
unsigned char conf_reg1;
unsigned char conf_reg2;
gyro_units_t units;
}TBNO055GyroConfig;
/* magnetometer definitions */
/* operation mode */
#define MAG_OP_MODE_MASK 0x18
#define MAG_LOW_POWER_MODE 0x00
#define MAG_REGULAR_MODE 0x08
#define MAG_EN_REGULAR_MODE 0x10
#define MAG_HICH_ACC_MODE 0x18
/* power mode */
#define MAG_POWER_MODE_MASK 0x60
#define MAG_NORMAL_MODE 0x00
#define MAG_SLEEP_MODE 0x20
#define MAG_SUSPEND_MODE 0x40
#define MAG_FORCE_MODE 0x60
/* rate */
#define MAG_RATE_MASK 0x07
#define MAG_2_RATE 0x00
#define MAG_6_RATE 0x01
#define MAG_8_RATE 0x02
#define MAG_10_RATE 0x03
#define MAG_15_RATE 0x04
#define MAG_20_RATE 0x05
#define MAG_25_RATE 0x06
#define MAG_30_RATE 0x07
typedef struct
{
unsigned char conf_reg;
}TBNO055MagConfig;
#define ACCEL_UNITS_MASK 0x01
#define GYRO_UNITS_MASK 0x02
#define EULER_UNITS_MASK 0x04
#define TEMP_UNITS_MASK 0x10
/* temperature definitions */
typedef enum {temp_celcius,temp_farenheit} temp_units_t;
/* axis remapping definitions */
typedef enum {X_AXIS=0x00,Y_AXIS=0x01,Z_AXIS=0x02} axis_t;
class CBNO055IMUDriver class CBNO055IMUDriver
{ {
private: private:
......
src/bno055_imu_sim.cpp 0 → 100644
+ 370
− 0
View file @ 8b7a66bb
#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 0 → 100644
+ 54
− 0
View file @ 8b7a66bb
#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
+ 5
− 0
View file @ 8b7a66bb
...@@ -2,3 +2,8 @@ ...@@ -2,3 +2,8 @@
ADD_EXECUTABLE(bno055_imu_driver_test bno055_imu_driver_test.cpp) ADD_EXECUTABLE(bno055_imu_driver_test bno055_imu_driver_test.cpp)
# link necessary libraries # link necessary libraries
TARGET_LINK_LIBRARIES(bno055_imu_driver_test bno055_imu_driver) 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
+ 1
− 1
View file @ 8b7a66bb
...@@ -14,7 +14,7 @@ int main(int argc, char *argv[]) ...@@ -14,7 +14,7 @@ int main(int argc, char *argv[])
std::vector<double> quat,euler,linear_acc,gravity; std::vector<double> quat,euler,linear_acc,gravity;
unsigned int i; unsigned int i;
imu.open("/dev/ttyUSB1"); imu.open("/dev/pts/23");
try{ try{
imu.load_calibration("bno055.cal"); imu.load_calibration("bno055.cal");
do{ do{
......
src/examples/bno055_imu_sim_test.cpp 0 → 100644
+ 19
− 0
View file @ 8b7a66bb
#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;
}
}
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