Skip to content
GitLab
Commit 80986fb8 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Optimized the driver to increase the output data rate.

parent 26c333d9
Hide whitespace changes
Inline Side-by-side
include/bno055_imu_driver.h
View file @ 80986fb8
......@@ -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);
......
src/CMakeLists.txt
View file @ 80986fb8
# 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)
......
src/bno055_imu_driver.cpp
View file @ 80986fb8
......@@ -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");
......
src/examples/bno055_imu_driver_test.cpp
View file @ 80986fb8
......@@ -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){
......
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