Commit 962273b1 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

When stopping, changed the mode after ending the thread.

When starting, changed the mode before starting the thread.
parent cb6bb41e
......@@ -473,17 +473,7 @@ void CBNO055IMUDriver::set_operation_mode(op_mode_t op_mode)
{
if(this->op_mode!=op_mode)
{
try{
this->imu_access.enter();
this->change_register_page(0x00);
this->write_registers(0x3D,1,(unsigned char *)&op_mode);
this->imu_access.exit();
}catch(CException &e){
this->imu_access.exit();
throw;
}
this->op_mode=op_mode;
if(this->op_mode==config_mode)
if(op_mode==config_mode)
{
if(this->thread_server->get_thread_state(this->data_thread_id)==starting ||
this->thread_server->get_thread_state(this->data_thread_id)==active)
......@@ -492,9 +482,29 @@ void CBNO055IMUDriver::set_operation_mode(op_mode_t op_mode)
this->thread_server->end_thread(this->data_thread_id);
this->event_server->reset_event(this->finish_thread_event_id);
}
try{
this->imu_access.enter();
this->change_register_page(0x00);
this->write_registers(0x3D,1,(unsigned char *)&op_mode);
this->imu_access.exit();
}catch(CException &e){
this->imu_access.exit();
throw;
}
this->op_mode=op_mode;
}
else
{
try{
this->imu_access.enter();
this->change_register_page(0x00);
this->write_registers(0x3D,1,(unsigned char *)&op_mode);
this->imu_access.exit();
}catch(CException &e){
this->imu_access.exit();
throw;
}
this->op_mode=op_mode;
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