diff --git a/src/bno055_imu_driver.cpp b/src/bno055_imu_driver.cpp
index 83172a7df7b882f9859d01ac09df4fe97691bef5..6159d77d3efec959a93affd548e651be90f367b1 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);
     }