Commit 80986fb8 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Optimized the driver to increase the output data rate.

parent 26c333d9
......@@ -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);
......
# 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)
......
......@@ -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");
......
......@@ -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