diff --git a/src/bno055_imu_ros_driver.cpp b/src/bno055_imu_ros_driver.cpp
index f1852fcc4fba8190d6c4f34d7f81ed7573525b34..e5a3b3fbdb0d06f5aa711938e6d66b6e8a68075a 100644
--- a/src/bno055_imu_ros_driver.cpp
+++ b/src/bno055_imu_ros_driver.cpp
@@ -91,10 +91,7 @@ bool Bno055ImuDriver::stopDriver(void)
 
 op_mode_t Bno055ImuDriver::get_operation_mode(void)
 {
-  if(this->imu_device!=NULL) 
-    return this->imu_device->get_operation_mode();
-  else
-    return config_mode;
+  return (op_mode_t)this->config_.mode;
 }
 
 void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level)
diff --git a/src/bno055_imu_ros_driver_node.cpp b/src/bno055_imu_ros_driver_node.cpp
index c89b5bf22d5c209f076bee956c7a8e931f2d19fe..47e23654133502d76e1acdca4d185936c568bca6 100644
--- a/src/bno055_imu_ros_driver_node.cpp
+++ b/src/bno055_imu_ros_driver_node.cpp
@@ -48,7 +48,7 @@ void Bno055ImuDriverNode::mainNodeThread(void)
     try{
       this->events.clear();
       this->events.push_back(this->driver_.get_new_data_event_id());
-      this->event_server->wait_all(events,2000);
+      this->event_server->wait_all(events,100);
       op_mode=this->driver_.get_operation_mode();
       if(op_mode==mag_only_mode || op_mode==acc_mag_mode || op_mode==mag_gyro_mode || op_mode==acc_mag_gyro_mode || op_mode==compass_mode || op_mode==m4g_mode || op_mode==ndof_off_mode || op_mode==ndof_mode)
       {