Commit 9cc6b0da authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

All data is read from the device whatever the operational mode.

parent a3f83cbd
......@@ -84,10 +84,20 @@ void CBNO055IMUDriver::convert_gyro_data(unsigned char *data)
void CBNO055IMUDriver::convert_quaternion_data(unsigned char *data)
{
this->quaternion_data[3]=((signed short int)(data[0]+(data[1]<<8)))/16384.0;
this->quaternion_data[0]=((signed short int)(data[2]+(data[3]<<8)))/16384.0;
this->quaternion_data[1]=((signed short int)(data[4]+(data[5]<<8)))/16384.0;
this->quaternion_data[2]=((signed short int)(data[6]+(data[7]<<8)))/16384.0;
if(this->op_mode>=imu_mode)
{
this->quaternion_data[3]=((signed short int)(data[0]+(data[1]<<8)))/16384.0;
this->quaternion_data[0]=((signed short int)(data[2]+(data[3]<<8)))/16384.0;
this->quaternion_data[1]=((signed short int)(data[4]+(data[5]<<8)))/16384.0;
this->quaternion_data[2]=((signed short int)(data[6]+(data[7]<<8)))/16384.0;
}
else
{
this->quaternion_data[3]=1.0;
this->quaternion_data[0]=0.0;
this->quaternion_data[1]=0.0;
this->quaternion_data[2]=0.0;
}
}
void CBNO055IMUDriver::convert_euler_angles_data(unsigned char *data)
......@@ -149,45 +159,20 @@ void CBNO055IMUDriver::change_register_page(unsigned char page_id)
void CBNO055IMUDriver::process_data(unsigned char *data)
{
this->data_access.enter();
switch(this->op_mode)
{
case config_mode: break;
case acc_only_mode: this->convert_accel_data(data);
break;
case mag_only_mode: this->convert_mag_data(data);
break;
case gyro_only_mode: this->convert_gyro_data(data);
break;
case acc_mag_mode: this->convert_accel_data(data);
this->convert_mag_data(&data[6]);
break;
case acc_gyro_mode: this->convert_accel_data(data);
this->convert_gyro_data(&data[12]);
break;
case mag_gyro_mode: this->convert_mag_data(data);
this->convert_gyro_data(&data[6]);
break;
case acc_mag_gyro_mode: this->convert_accel_data(data);
this->convert_mag_data(&data[6]);
this->convert_gyro_data(&data[12]);
break;
case imu_mode:
case compass_mode:
case m4g_mode:
case ndof_off_mode:
case ndof_mode: this->convert_euler_angles_data(data);
this->convert_quaternion_data(&data[6]);
this->convert_linear_accel_data(&data[14]);
this->convert_gravity_data(&data[20]);
break;
}
this->convert_accel_data(data);
this->convert_mag_data(&data[6]);
this->convert_gyro_data(&data[12]);
this->convert_euler_angles_data(&data[18]);
this->convert_quaternion_data(&data[24]);
this->convert_linear_accel_data(&data[32]);
this->convert_gravity_data(&data[38]);
this->data_access.exit();
}
void *CBNO055IMUDriver::data_thread(void *param)
{
CBNO055IMUDriver *imu=(CBNO055IMUDriver *)param;
unsigned char address,length,*data;
unsigned char address=0x08,length=44,data[44];
bool end=false;
while(!end)
......@@ -196,49 +181,7 @@ void *CBNO055IMUDriver::data_thread(void *param)
end=true;
else
{
switch(imu->op_mode)
{
case config_mode: address=0x00;
length=0x00;
break;
case acc_only_mode: address=0x08;
length=6;
data=new unsigned char[length];
break;
case mag_only_mode: address=0x0E;
length=6;
data=new unsigned char[length];
break;
case gyro_only_mode: address=0x14;
length=6;
data=new unsigned char[length];
break;
case acc_mag_mode: address=0x08;
length=12;
data=new unsigned char[length];
break;
case acc_gyro_mode: address=0x08;
length=18;
data=new unsigned char[length];
break;
case mag_gyro_mode: address=0x0E;
length=12;
data=new unsigned char[length];
break;
case acc_mag_gyro_mode: address=0x08;
length=18;
data=new unsigned char[length];
break;
case imu_mode:
case compass_mode:
case m4g_mode:
case ndof_off_mode:
case ndof_mode: address=0x1A;
length=26;
data=new unsigned char[length];
break;
}
if(length>0)
if(imu->op_mode!=config_mode)
{
try{
imu->imu_access.enter();
......@@ -246,14 +189,10 @@ void *CBNO055IMUDriver::data_thread(void *param)
imu->read_registers(address,length,data);
imu->imu_access.exit();
imu->process_data(data);
delete[] 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();
delete[] data;
std::cout << e.what() << std::endl;
}
}
......@@ -555,62 +494,6 @@ void CBNO055IMUDriver::set_operation_mode(op_mode_t op_mode)
}
else
{
switch(op_mode)
{
case config_mode: break;
case acc_only_mode: this->raw_mag_data=std::vector<double>(3,0.0);
this->raw_gyro_data=std::vector<double>(3,0.0);
this->quaternion_data=std::vector<double>(3,0.0);
this->euler_angles_data=std::vector<double>(3,0.0);
this->linear_accel_data=std::vector<double>(3,0.0);
this->gravity_data=std::vector<double>(3,0.0);
break;
case mag_only_mode: this->raw_accel_data=std::vector<double>(3,0.0);
this->raw_gyro_data=std::vector<double>(3,0.0);
this->quaternion_data=std::vector<double>(3,0.0);
this->euler_angles_data=std::vector<double>(3,0.0);
this->linear_accel_data=std::vector<double>(3,0.0);
this->gravity_data=std::vector<double>(3,0.0);
break;
case gyro_only_mode: this->raw_accel_data=std::vector<double>(3,0.0);
this->raw_mag_data=std::vector<double>(3,0.0);
this->quaternion_data=std::vector<double>(3,0.0);
this->euler_angles_data=std::vector<double>(3,0.0);
this->linear_accel_data=std::vector<double>(3,0.0);
this->gravity_data=std::vector<double>(3,0.0);
break;
case acc_mag_mode: this->raw_gyro_data=std::vector<double>(3,0.0);
this->quaternion_data=std::vector<double>(3,0.0);
this->euler_angles_data=std::vector<double>(3,0.0);
this->linear_accel_data=std::vector<double>(3,0.0);
this->gravity_data=std::vector<double>(3,0.0);
break;
case acc_gyro_mode: this->raw_mag_data=std::vector<double>(3,0.0);
this->quaternion_data=std::vector<double>(3,0.0);
this->euler_angles_data=std::vector<double>(3,0.0);
this->linear_accel_data=std::vector<double>(3,0.0);
this->gravity_data=std::vector<double>(3,0.0);
break;
case mag_gyro_mode: this->raw_accel_data=std::vector<double>(3,0.0);
this->quaternion_data=std::vector<double>(3,0.0);
this->euler_angles_data=std::vector<double>(3,0.0);
this->linear_accel_data=std::vector<double>(3,0.0);
this->gravity_data=std::vector<double>(3,0.0);
break;
case acc_mag_gyro_mode: this->quaternion_data=std::vector<double>(3,0.0);
this->euler_angles_data=std::vector<double>(3,0.0);
this->linear_accel_data=std::vector<double>(3,0.0);
this->gravity_data=std::vector<double>(3,0.0);
break;
case imu_mode:
case compass_mode:
case m4g_mode:
case ndof_off_mode:
case ndof_mode: this->raw_accel_data=std::vector<double>(3,0.0);
this->raw_mag_data=std::vector<double>(3,0.0);
this->raw_gyro_data=std::vector<double>(3,0.0);
break;
}
if(this->thread_server->get_thread_state(this->data_thread_id)==attached)
this->thread_server->start_thread(this->data_thread_id);
}
......
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