diff --git a/include/bno055_imu_driver.h b/include/bno055_imu_driver.h
index f69a302197e3e4ca7b5940e4f15d55b9b73686de..a68d95249b31fd6de6dc18f7e1a300b6275ea9ae 100644
--- a/include/bno055_imu_driver.h
+++ b/include/bno055_imu_driver.h
@@ -69,6 +69,9 @@ class CBNO055IMUDriver
void detect_sensor(void);
void get_sensor_units(void);
void get_sensor_configs(void);
+ void send_read_cmd(unsigned char address,unsigned char length);
+ void send_write_cmd(unsigned char address,unsigned char length,unsigned char *data);
+ void get_answer(unsigned char length,unsigned char *data);
public:
CBNO055IMUDriver(const std::string &name);
void open(const std::string &serial_dev);
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 865a085bb606ed52b35bafda4eb0cedee6861a0a..85e9aa68b824758fdc1fbc3ef67b978a760db112 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,4 +1,6 @@
# driver source files
+add_compile_options(-std=c++11)
+
SET(sources bno055_imu_driver.cpp bno055_imu_exceptions.cpp)
# application header files
SET(headers ../include/bno055_imu_driver.h ../include/bno055_imu_exceptions.h ../include/bno055_common.h)
diff --git a/src/bno055_imu_driver.cpp b/src/bno055_imu_driver.cpp
index 1475131140bcb7d48a87096b894f785a3a223ba8..44117d231e810ef92a81146fac60c5f6ddc32cc8 100644
--- a/src/bno055_imu_driver.cpp
+++ b/src/bno055_imu_driver.cpp
@@ -212,27 +212,28 @@ void *CBNO055IMUDriver::data_thread(void *param)
while(!end)
{
- if(imu->event_server->event_is_set(imu->finish_thread_event_id))
- end=true;
- else
+ if(imu->op_mode!=config_mode)
{
- if(imu->op_mode!=config_mode)
- {
- try{
- imu->imu_access.enter();
- imu->change_register_page(0x00);
- imu->read_registers(address,length,data);
- imu->imu_access.exit();
- imu->process_data(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;
- }
+ try{
+ imu->imu_access.enter();
+ imu->send_read_cmd(address,length);
+ imu->imu_access.exit();
+ usleep((unsigned long int)(1000000.0/imu->data_rate_hz)-1000);
+ imu->imu_access.enter();
+ imu->get_answer(length,data);
+ imu->imu_access.exit();
+ imu->process_data(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;
}
}
- usleep((unsigned long int)(1000000.0/imu->data_rate_hz));
+ else
+ usleep((unsigned long int)(1000000.0/imu->data_rate_hz));
+ if(imu->event_server->event_is_set(imu->finish_thread_event_id))
+ end=true;
}
pthread_exit(NULL);
@@ -240,50 +241,37 @@ void *CBNO055IMUDriver::data_thread(void *param)
void CBNO055IMUDriver::write_registers(unsigned char address, unsigned char length,unsigned char *data)
{
- unsigned char *cmd,answer[2];
+ unsigned char answer[2];
std::list<std::string> events;
- unsigned int read_data=0,num,i,retries=0;
+ unsigned int read_data=0,num;
if(this->imu_port==NULL)
throw CBNO055IMUException(_HERE_,"Communications device not ready");
- cmd=new unsigned char[length+4];
- cmd[0]=0xAA;
- cmd[1]=0x00;
- cmd[2]=address;
- cmd[3]=length;
- for(i=0;i<length;i++)
- cmd[4+i]=data[i];
+ this->send_write_cmd(address,length,data);
events.push_back(this->imu_port->get_rx_event_id());
- do{
- this->imu_port->write(cmd,length+4);
- try{
- /* read the header */
- while(read_data<2)
- {
- if((num=this->imu_port->get_num_data())==0)
- {
- this->event_server->wait_all(events,500);
- num=this->imu_port->get_num_data();
- }
- if(num>(2-read_data))
- {
- this->imu_port->read(&answer[read_data],2-read_data);
- read_data+=(2-read_data);
- }
- else
- {
- this->imu_port->read(&answer[read_data],num);
- read_data+=num;
- }
+ try{
+ /* read the header */
+ while(read_data<2)
+ {
+ if((num=this->imu_port->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,500);
+ num=this->imu_port->get_num_data();
+ }
+ if(num>(2-read_data))
+ {
+ this->imu_port->read(&answer[read_data],2-read_data);
+ read_data+=(2-read_data);
+ }
+ else
+ {
+ this->imu_port->read(&answer[read_data],num);
+ read_data+=num;
}
- }catch(CEventTimeoutException &e){
- delete[] cmd;
- throw CBNO055IMUException(_HERE_,"Write acknowledge not received in the allowed time");
}
- retries++;
- read_data=0;
- }while(answer[1]!=0x01 && retries<5);
- delete[] cmd;
+ }catch(CEventTimeoutException &e){
+ throw CBNO055IMUException(_HERE_,"Write acknowledge not received in the allowed time");
+ }
switch(answer[1])
{
case 0x01: return;
@@ -301,46 +289,39 @@ void CBNO055IMUDriver::write_registers(unsigned char address, unsigned char leng
void CBNO055IMUDriver::read_registers(unsigned char address, unsigned char length,unsigned char *data)
{
- unsigned char cmd[4],answer[2];
+ unsigned char answer[2];
std::list<std::string> events;
- unsigned int read_data=0,num,retries=0;
+ unsigned int read_data=0,num;
if(this->imu_port==NULL)
throw CBNO055IMUException(_HERE_,"Communications device not ready");
- cmd[0]=0xAA;
- cmd[1]=0x01;
- cmd[2]=address;
- cmd[3]=length;
- events.push_back(this->imu_port->get_rx_event_id());
/* send the command */
- do{
- this->imu_port->write(cmd,4);
- try{
- /* read the header */
- while(read_data<2)
- {
- if((num=this->imu_port->get_num_data())==0)
- {
- this->event_server->wait_all(events,500);
- num=this->imu_port->get_num_data();
- }
- if(num>(2-read_data))
- {
- this->imu_port->read(&answer[read_data],2-read_data);
- read_data+=(2-read_data);
- }
- else
- {
- this->imu_port->read(&answer[read_data],num);
- read_data+=num;
- }
+ this->send_read_cmd(address,length);
+ events.push_back(this->imu_port->get_rx_event_id());
+ try{
+ /* read the header */
+ while(read_data<2)
+ {
+ if((num=this->imu_port->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,500);
+ num=this->imu_port->get_num_data();
+ }
+ if(num>(2-read_data))
+ {
+ this->imu_port->read(&answer[read_data],2-read_data);
+ read_data+=(2-read_data);
+ }
+ else
+ {
+ this->imu_port->read(&answer[read_data],num);
+ read_data+=num;
}
- }catch(CEventTimeoutException &e){
- throw CBNO055IMUException(_HERE_,"Expected data not received in the allowed time");
}
- retries++;
- read_data=0;
- }while(answer[0]!=0xBB && retries<5);
+ }catch(CEventTimeoutException &e){
+ throw CBNO055IMUException(_HERE_,"Expected data not received in the allowed time");
+ }
+ read_data=0;
if(answer[0]==0xEE)/* there has been an error */
{
switch(answer[1])
@@ -382,6 +363,124 @@ void CBNO055IMUDriver::read_registers(unsigned char address, unsigned char lengt
}
}
+void CBNO055IMUDriver::send_read_cmd(unsigned char address,unsigned char length)
+{
+ unsigned char cmd;
+
+ if(this->imu_port==NULL)
+ throw CBNO055IMUException(_HERE_,"Communications device not ready");
+ cmd=0xAA;
+ this->imu_port->write(&cmd,1);
+ usleep(250);
+ cmd=0x01;
+ this->imu_port->write(&cmd,1);
+ usleep(250);
+ cmd=address;
+ this->imu_port->write(&cmd,1);
+ usleep(250);
+ cmd=length;
+ this->imu_port->write(&cmd,1);
+ usleep(250);
+}
+
+void CBNO055IMUDriver::send_write_cmd(unsigned char address,unsigned char length,unsigned char *data)
+{
+ unsigned char cmd;
+
+ if(this->imu_port==NULL)
+ throw CBNO055IMUException(_HERE_,"Communications device not ready");
+ cmd=0xAA;
+ this->imu_port->write(&cmd,1);
+ usleep(250);
+ cmd=0x00;
+ this->imu_port->write(&cmd,1);
+ usleep(250);
+ cmd=address;
+ this->imu_port->write(&cmd,1);
+ usleep(250);
+ cmd=length;
+ this->imu_port->write(&cmd,1);
+ usleep(250);
+ for(unsigned int i=0;i<length;i++)
+ {
+ cmd=data[i];
+ this->imu_port->write(&cmd,1);
+ usleep(250);
+ }
+}
+
+void CBNO055IMUDriver::get_answer(unsigned char length,unsigned char *data)
+{
+ unsigned char answer[2];
+ std::list<std::string> events;
+ unsigned int read_data=0,num;
+
+ events.push_back(this->imu_port->get_rx_event_id());
+ try{
+ /* read the header */
+ while(read_data<2)
+ {
+ if((num=this->imu_port->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,500);
+ num=this->imu_port->get_num_data();
+ }
+ if(num>(2-read_data))
+ {
+ this->imu_port->read(&answer[read_data],2-read_data);
+ read_data+=(2-read_data);
+ }
+ else
+ {
+ this->imu_port->read(&answer[read_data],num);
+ read_data+=num;
+ }
+ }
+ }catch(CEventTimeoutException &e){
+ throw CBNO055IMUException(_HERE_,"Expected data not received in the allowed time");
+ }
+ read_data=0;
+ if(answer[0]==0xEE)/* there has been an error */
+ {
+ switch(answer[1])
+ {
+ case 0x02: throw CBNO055IMUException(_HERE_,"Impossible to execute the read operation");
+ case 0x04: throw CBNO055IMUException(_HERE_,"Invalid address");
+ case 0x05: throw CBNO055IMUException(_HERE_,"Write operation disabled");
+ case 0x06: throw CBNO055IMUException(_HERE_,"Invalid start byte");
+ case 0x07: throw CBNO055IMUException(_HERE_,"Serial overrun");
+ case 0x08: throw CBNO055IMUException(_HERE_,"Maximum length exceeded");
+ case 0x09: throw CBNO055IMUException(_HERE_,"Invalid command");
+ case 0x0A: throw CBNO055IMUException(_HERE_,"Data timeout");
+ }
+ }
+ try{
+ /* read the received data */
+ while(read_data<(int)answer[1])
+ {
+ if((num=this->imu_port->get_num_data())==0)
+ {
+ this->event_server->wait_all(events,500);
+ num=this->imu_port->get_num_data();
+ }
+ if(num>(length-read_data))
+ {
+ this->imu_port->read(&data[read_data],length-read_data);
+ read_data+=(length-read_data);
+ }
+ else
+ {
+ this->imu_port->read(&data[read_data],num);
+ read_data+=num;
+ }
+ }
+ if(answer[1]!=length)
+ throw CBNO055IMUException(_HERE_,"Not all data has been received");
+ }catch(CEventTimeoutException &e){
+ throw CBNO055IMUException(_HERE_,"Expected data not received in the allowed time");
+ }
+}
+
void CBNO055IMUDriver::detect_sensor(void)
{
try{
@@ -681,7 +780,7 @@ void CBNO055IMUDriver::get_remap_axis(std::vector<char> &new_x,std::vector<char>
void CBNO055IMUDriver::set_data_rate(double rate_hz)
{
- if(rate_hz>0.0 && rate_hz<=100.0)
+ if(rate_hz>0.0 && rate_hz<=1000.0)
this->data_rate_hz=rate_hz;
else
throw CBNO055IMUException(_HERE_,"Invalid data rate");
diff --git a/src/examples/bno055_imu_driver_test.cpp b/src/examples/bno055_imu_driver_test.cpp
index 55498fcd4009fffb7b9e394d201c9f0e561e43f2..bcfaf2315ff86e6965a2ce40777a64bdde4eaa11 100644
--- a/src/examples/bno055_imu_driver_test.cpp
+++ b/src/examples/bno055_imu_driver_test.cpp
@@ -2,6 +2,7 @@
#include "bno055_imu_exceptions.h"
#include <unistd.h>
#include <iostream>
+#include <chrono>
int main(int argc, char *argv[])
{
@@ -13,25 +14,40 @@ int main(int argc, char *argv[])
std::vector<double> accel,mag,gyro;
std::vector<double> quat,euler,linear_acc,gravity;
unsigned int i;
+ unsigned char range,bandwidth,mode,rate,op_mode,power_mode;
- imu.open("/tmp/darwin1_imu_uart");
+ imu.open("/dev/ttyUSB2");
try{
imu.load_calibration("bno055.cal");
}catch(CException &e){
std::cout << e.what() << std::endl;
}
- 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;
+// do{
+// 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;
+// }while(!accel_cal || ! mag_cal || !gyro_cal);
events.push_back(imu.get_new_data_event_id());
+ imu.get_accel_config(&range,&bandwidth,&mode);
+ bandwidth=ACCEL_125_BW;
+ imu.set_accel_config(range,bandwidth,mode);
+ imu.get_mag_config(&rate,&op_mode,&power_mode);
+ rate=MAG_30_RATE;
+ imu.set_mag_config(rate,op_mode,power_mode);
+ imu.get_gyro_config(&range,&bandwidth,&mode);
+ bandwidth=GYRO_116_BW;
+ imu.set_gyro_config(range,bandwidth,mode);
+ imu.set_operation_mode(acc_mag_gyro_mode);
+ imu.set_data_rate(100);
for(i=0;i<1000;i++)
{
+ auto t1 = std::chrono::high_resolution_clock::now();
event_server->wait_all(events,1000);
+/*
quat=imu.get_orientation_quat();
std::cout << "Quaternion: X: " << quat[0] << ", Y: " << quat[1] << ", Z: " << quat[2] << ", W: " << quat[3] << std::endl;
euler=imu.get_orientation_euler();
@@ -46,6 +62,11 @@ int main(int argc, char *argv[])
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;
+*/
+ auto t2 = std::chrono::high_resolution_clock::now();
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>( t2 - t1 ).count();
+ std::cout << duration << std::endl;
+ std::cout << std::flush;
}
}catch(CException &e){