diff --git a/Findbno055_imu_sim.cmake b/Findbno055_imu_sim.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..d535c23e11c081b8d798698db82b51dee5f9e9d0
--- /dev/null
+++ b/Findbno055_imu_sim.cmake
@@ -0,0 +1,21 @@
+#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 3f9444e484052ee94b4ca863d630caa895c6aa6c..b7b96683553aa06999ff893d498c587e14bd3ad5 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,7 +1,11 @@
# driver source files
SET(sources bno055_imu_driver.cpp bno055_imu_exceptions.cpp)
# 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
FIND_PACKAGE(iriutils REQUIRED)
FIND_PACKAGE(comm REQUIRED)
@@ -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 ${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_common.h b/src/bno055_common.h
new file mode 100644
index 0000000000000000000000000000000000000000..762e91bc6502ecf0f3826e8ded22038b53a44fa9
--- /dev/null
+++ b/src/bno055_common.h
@@ -0,0 +1,148 @@
+#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
diff --git a/src/bno055_imu_driver.h b/src/bno055_imu_driver.h
index 3b81feaaed596e8a63e075076b9ceb4e588b4b31..2ddb42401e89e1734b818bbc00eb301ec56f3edf 100644
--- a/src/bno055_imu_driver.h
+++ b/src/bno055_imu_driver.h
@@ -4,152 +4,9 @@
#include "rs232.h"
#include "mutex.h"
#include "threadserver.h"
+#include "bno055_common.h"
#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
{
private:
diff --git a/src/bno055_imu_sim.cpp b/src/bno055_imu_sim.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d4d0291f9ed77c8c1aee65162985da19c204a45d
--- /dev/null
+++ b/src/bno055_imu_sim.cpp
@@ -0,0 +1,370 @@
+#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
new file mode 100644
index 0000000000000000000000000000000000000000..0628c3265c87957a524b5f4e9b021b2aa7416219
--- /dev/null
+++ b/src/bno055_imu_sim.h
@@ -0,0 +1,54 @@
+#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 fd72ae5a9b0a36a24c83a0ca340577a11a071117..586bfd62897e2bfbc341b5ce3d6b2b20c68da8e1 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -2,3 +2,8 @@
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 7553f9a90cc9fe4dd82c4992fd19090cda2bdb36..d5722fe25b428aa81feaefa8f232943e817432f0 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/ttyUSB1");
+ imu.open("/dev/pts/23");
try{
imu.load_calibration("bno055.cal");
do{
diff --git a/src/examples/bno055_imu_sim_test.cpp b/src/examples/bno055_imu_sim_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1c783f49ffd44956620e0b20de567b1ea486fd85
--- /dev/null
+++ b/src/examples/bno055_imu_sim_test.cpp
@@ -0,0 +1,19 @@
+#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;
+ }
+}