From 962273b10bf8c4c1607dbb25229cf1bc04c82820 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 4 Feb 2020 09:11:46 +0100
Subject: [PATCH] When stopping, changed the mode after ending the thread. When
starting, changed the mode before starting the thread.
---
src/bno055_imu_driver.cpp | 32 +++++++++++++++++++++-----------
1 file changed, 21 insertions(+), 11 deletions(-)
diff --git a/src/bno055_imu_driver.cpp b/src/bno055_imu_driver.cpp
index 83172a7..6159d77 100644
--- a/src/bno055_imu_driver.cpp
+++ b/src/bno055_imu_driver.cpp
@@ -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);
}
--
GitLab