diff --git a/CMakeLists.txt b/CMakeLists.txt
index e1e039a8534f25fe45b6ad2d91b5324042316cbd..a25d5c1adb3c2a64ca0eb5b61a86858e4c7b2ab8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(bno055_imu)
+project(iri_bno055_imu_driver)
 
 ## Find catkin macros and libraries
 find_package(catkin REQUIRED)
@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS iri_base_driver sensor_msgs)
 #           Add system and labrobotica dependencies here
 # ******************************************************************** 
 find_package(iriutils REQUIRED)
+find_package(comm REQUIRED)
 find_package(bno055_imu_driver REQUIRED)
 # find_package(<dependency> REQUIRED)
 
@@ -79,6 +80,7 @@ catkin_package(
 include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
 include_directories(${iriutils_INCLUDE_DIR})
+include_directories(${comm_INCLUDE_DIR})
 include_directories(${bno055_imu_driver_INCLUDE_DIR})
 # include_directories(${<dependency>_INCLUDE_DIR})
 
@@ -93,6 +95,7 @@ add_executable(${PROJECT_NAME} src/bno055_imu_ros_driver.cpp src/bno055_imu_ros_
 # ******************************************************************** 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${comm_LIBRARY})
 target_link_libraries(${PROJECT_NAME} ${bno055_imu_driver_LIBRARY})
 # target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
 
@@ -101,6 +104,7 @@ target_link_libraries(${PROJECT_NAME} ${bno055_imu_driver_LIBRARY})
 # ******************************************************************** 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME}_gencfg iri_base_driver_generate_messages_cpp)
 # ******************************************************************** 
 #               Add dynamic reconfigure dependencies 
 # ******************************************************************** 
diff --git a/calibration/bno055.cal b/calibration/bno055.cal
deleted file mode 100644
index 415824d33b03a35d9afda9fda66eb71dd64af48e..0000000000000000000000000000000000000000
Binary files a/calibration/bno055.cal and /dev/null differ
diff --git a/cfg/Bno055Imu.cfg b/cfg/Bno055Imu.cfg
index 5a4c162257aafa1cec0870d650848b066996cb16..c52f99dabb131035e90d76e7381654f1eec7a840 100755
--- a/cfg/Bno055Imu.cfg
+++ b/cfg/Bno055Imu.cfg
@@ -31,19 +31,121 @@
 
 # Author: 
 
-PACKAGE='bno055_imu'
+PACKAGE='iri_bno055_imu_driver'
 
-from driver_base.msg import SensorLevels
+from iri_base_driver.msg import SensorLevels
 from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
+enum_operation_mode = gen.enum([ 
+gen.const("accel_only",  int_t, 1, "Only enable the accelerometer"),
+gen.const("mag_only",  int_t, 2, "Only enable the magnetometer"),
+gen.const("gyro_only", int_t, 3, "Only enable the gyroscope"),
+gen.const("accel_mag", int_t, 4, "Enable both accelerometer and magnetometer"),
+gen.const("accel_gyro", int_t, 5, "Enable both accelerometer and gyroscope"),
+gen.const("mag_gyro", int_t, 6, "Enable both magnetometer and gyroscope"),
+gen.const("accel_mag_gyro", int_t, 7, "Enable all sensors"),
+gen.const("imu", int_t, 8, "IMU"),
+gen.const("compass", int_t, 9, "Compass"),
+gen.const("m4g", int_t, 10, "M4G"),
+gen.const("ndof_off", int_t, 11, "ndof off"),
+gen.const("ndof", int_t, 12, "ndof")
+], "Possible operation modes.")
+
+accel = gen.add_group("Accelerometer")
+accel_range = gen.enum([ 
+gen.const("2G", int_t, 0, "+/- 2G"),
+gen.const("4G", int_t, 1, "+/- 4G"),
+gen.const("8G", int_t, 2, "+/- 8G"),
+gen.const("16G", int_t, 3, "+/- 16G")
+], "Accelerometer ranges.")
+accel_bw = gen.enum([ 
+gen.const("7_81Hz", int_t, 0, "7.81Hz"),
+gen.const("15_63Hz", int_t, 4, "15.63Hz"),
+gen.const("32_25Hz", int_t, 8, "32.25Hz"),
+gen.const("62_5Hz", int_t, 12, "62.5Hz"),
+gen.const("125Hz", int_t, 16, "125Hz"),
+gen.const("250Hz", int_t, 20, "250Hz"),
+gen.const("500Hz", int_t, 24, "500Hz"),
+gen.const("1000Hz", int_t, 28, "1000Hz")
+], "Accelerometer bandwidth.")
+accel_pwr_mode = gen.enum([ 
+gen.const("accel_normal", int_t, 0, "Normal"),
+gen.const("accel_suspend", int_t, 32, "Suspend"),
+gen.const("accel_low_pwr1", int_t, 64, "Low power 1"),
+gen.const("accel_standby", int_t, 96, "Stand by"),
+gen.const("accel_low_pwr2", int_t, 128, "Low power 2"),
+gen.const("accel_deep_suspend", int_t, 160, "Seep suspend")
+], "Accelerometer power modes.")
+
+gyro = gen.add_group("Gyroscope")
+gyro_range = gen.enum([ 
+gen.const("2000dps", int_t, 0, "+/- 2000 dps"),
+gen.const("1000dps", int_t, 1, "+/- 1000 dps"),
+gen.const("500dps", int_t, 2, "+/- 500 dps"),
+gen.const("250dps", int_t, 3, "+/- 250 dps"),
+gen.const("124dps", int_t, 4, "+/- 125 dps")
+], "Gyroscope ranges.")
+gyro_bw = gen.enum([ 
+gen.const("523Hz", int_t, 0, "523Hz"),
+gen.const("230Hz", int_t, 8, "230Hz"),
+gen.const("116Hz", int_t, 16, "116Hz"),
+gen.const("47Hz", int_t, 24, "47Hz"),
+gen.const("23Hz", int_t, 32, "23Hz"),
+gen.const("12Hz", int_t, 40, "12Hz"),
+gen.const("64Hz", int_t, 48, "64Hz"),
+gen.const("32Hz", int_t, 56, "32Hz")
+], "Gyroscope bandwidth.")
+gyro_pwr_mode = gen.enum([ 
+gen.const("gyro_normal", int_t, 0, "Normal"),
+gen.const("gyro_fast_pwr_up", int_t, 1, "Fast power up"),
+gen.const("gyro_deep_suspend", int_t, 2, "Deep suspend"),
+gen.const("gyro_suspend", int_t, 3, "Suspend"),
+gen.const("gyro_adv_pwr_save", int_t, 4, "Advanced power save"),
+], "Gyroscope power modes.")
+
+mag = gen.add_group("Magnetometer")
+mag_rate = gen.enum([ 
+gen.const("2Hz", int_t, 0, "2 Hz"),
+gen.const("6Hz", int_t, 1, "6 Hz"),
+gen.const("8Hz", int_t, 2, "8 Hz"),
+gen.const("10Hz", int_t, 3, "10 Hz"),
+gen.const("15Hz", int_t, 4, "15 Hz"),
+gen.const("20Hz", int_t, 5, "20 Hz"),
+gen.const("25Hz", int_t, 6, "25 Hz"),
+gen.const("30Hz", int_t, 7, "30 Hz")
+], "Magnetometer rates.")
+mag_op_mode = gen.enum([ 
+gen.const("low_pwr", int_t, 0, "Low power"),
+gen.const("regular", int_t, 8, "Regular"),
+gen.const("en_regular", int_t, 16, "Enhanced regular"),
+gen.const("high_acc", int_t, 24, "High accuracy")
+], "Magnetometer operation moes.")
+mag_pwr_mode = gen.enum([ 
+gen.const("mag_normal", int_t, 0, "Normal"),
+gen.const("mag_sleep", int_t, 32, "Sleep"),
+gen.const("mag_suspend", int_t, 64, "Suspend"),
+gen.const("mag_force_mode", int_t, 96, "Force mode"),
+], "Magnetometer power modes.")
+
+
 #       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
 gen.add("update_rate",             double_t,  SensorLevels.RECONFIGURE_STOP,   "Data update rate in Hz",         20.0,     1.0,  100.0)
-gen.add("serial_device",           str_t,     SensorLevels.RECONFIGURE_STOP,   "Device serial port",             "/dev/ttyUSB0")
+gen.add("serial_device",           str_t,     SensorLevels.RECONFIGURE_CLOSE,  "Device serial port",             "/dev/ttyUSB1")
 gen.add("cal_filename",            str_t,     SensorLevels.RECONFIGURE_STOP,   "Sensor calibration data",        "")
 gen.add("tf_prefix",               str_t,     SensorLevels.RECONFIGURE_STOP,   "TF prefix",                      "")
 gen.add("frame_id",                str_t,     SensorLevels.RECONFIGURE_STOP,   "IMU frame_id in the urdf file",  "")
+gen.add("mode",                    int_t,     SensorLevels.RECONFIGURE_STOP,   "Operation mode",                 12,       1,    12, edit_method=enum_operation_mode)
+accel.add("accel_range",           int_t,     SensorLevels.RECONFIGURE_STOP,   "Accelerometer operation range",  1,        0,    3, edit_method=accel_range)
+accel.add("accel_bandwidth",       int_t,     SensorLevels.RECONFIGURE_STOP,   "Accelerometer bandwidth",        12,       0,    28, edit_method=accel_bw)
+accel.add("accel_pwr_mode",        int_t,     SensorLevels.RECONFIGURE_STOP,   "Accelerometer power mode",       0,        0,    160, edit_method=accel_pwr_mode)
+gyro.add("gyro_range",             int_t,     SensorLevels.RECONFIGURE_STOP,   "Gyroscope operation range",      0,        0,    4, edit_method=gyro_range)
+gyro.add("gyro_bandwidth",         int_t,     SensorLevels.RECONFIGURE_STOP,   "Gyroscope bandwidth",            56,       0,    56, edit_method=gyro_bw)
+gyro.add("gyro_pwr_mode",          int_t,     SensorLevels.RECONFIGURE_STOP,   "Gyroscope power mode",           0,        0,    4, edit_method=gyro_pwr_mode)
+gyro.add("mag_rate",               int_t,     SensorLevels.RECONFIGURE_STOP,   "Magnetometer rate",              3,        0,    7, edit_method=mag_rate)
+gyro.add("mag_op_mode",            int_t,     SensorLevels.RECONFIGURE_STOP,   "Magnetometer operation mode",    1,        0,    24, edit_method=mag_op_mode)
+gyro.add("mag_pwr_mode",           int_t,     SensorLevels.RECONFIGURE_STOP,   "Magnetometer power mode",        0,        0,    96, edit_method=mag_pwr_mode)
 #gen.add("velocity_scale_factor",  double_t,  SensorLevels.RECONFIGURE_STOP,   "Maximum velocity scale factor",  0.5,      0.0,  1.0)
 
 exit(gen.generate(PACKAGE, "Bno055ImuDriver", "Bno055Imu"))
diff --git a/include/bno055_imu_ros_driver.h b/include/bno055_imu_ros_driver.h
index 934d6e2973852b6e81062bebae2c52b320c4fcc1..aa9a6bb0a32940de0b4273af7b7fec378bb38fb5 100644
--- a/include/bno055_imu_ros_driver.h
+++ b/include/bno055_imu_ros_driver.h
@@ -26,7 +26,7 @@
 #define _bno055_imu_driver_h_
 
 #include <iri_base_driver/iri_base_driver.h>
-#include <bno055_imu/Bno055ImuConfig.h>
+#include <iri_bno055_imu_driver/Bno055ImuConfig.h>
 
 //include bno055_imu_driver main library
 #include "bno055_imu_driver.h"
@@ -64,7 +64,7 @@ class Bno055ImuDriver : public iri_base_driver::IriBaseDriver
     * Define a Config type with the Bno055ImuConfig. All driver implementations
     * will then use the same variable type Config.
     */
-    typedef bno055_imu::Bno055ImuConfig Config;
+    typedef iri_bno055_imu_driver::Bno055ImuConfig Config;
 
    /**
     * \brief config variable
@@ -148,10 +148,14 @@ class Bno055ImuDriver : public iri_base_driver::IriBaseDriver
     // here define all bno055_imu_driver interface methods to retrieve and set
     // the driver parameters
     std::string get_new_data_event_id(void);
+    op_mode_t get_operation_mode(void);
     std::vector<double> get_orientation_quat(void);
     std::vector<double> get_orientation_euler(void);
     std::vector<double> get_linear_accel(void);
     std::vector<double> get_gravity(void);
+    std::vector<double> get_raw_accelerometer(void);
+    std::vector<double> get_raw_magnetometer(void);
+    std::vector<double> get_raw_gyroscope(void);
     bool is_imu_calibrated(void);
     std::string get_frame_id(void);
    /**
diff --git a/include/bno055_imu_ros_driver_node.h b/include/bno055_imu_ros_driver_node.h
index 66ab1b0d47ab78da580f6b82f805c3a5408c283d..d4b7a9af8716af08bfc717a34aded16912f4e866 100644
--- a/include/bno055_imu_ros_driver_node.h
+++ b/include/bno055_imu_ros_driver_node.h
@@ -29,6 +29,7 @@
 #include "bno055_imu_ros_driver.h"
 
 // [publisher subscriber headers]
+#include <sensor_msgs/MagneticField.h>
 #include <sensor_msgs/Imu.h>
 
 // [service client headers]
@@ -58,6 +59,9 @@ class Bno055ImuDriverNode : public iri_base_driver::IriBaseNodeDriver<Bno055ImuD
 {
   private:
     // [publisher attributes]
+    ros::Publisher magnetometer_publisher_;
+    sensor_msgs::MagneticField magnetometer_MagneticField_msg_;
+
     ros::Publisher imu_publisher_;
     sensor_msgs::Imu imu_Imu_msg_;
 
diff --git a/launch/bno055_imu.launch b/launch/bno055_imu.launch
deleted file mode 100644
index b9d85ad4cc5a5ad9a6aa05e98872917b5a05f504..0000000000000000000000000000000000000000
--- a/launch/bno055_imu.launch
+++ /dev/null
@@ -1,16 +0,0 @@
-<launch>
-
-  <!-- launch the action client node -->
-  <node name="bno055_imu"
-        pkg="bno055_imu"
-        type="bno055_imu"
-        output="screen">
-    <param name="update_rate" value="1"/>
-    <param name="serial_device" value="/dev/ttyUSB1"/>
-    <param name="cal_filename" value="$(find bno055_imu)/calibration/bno055.cal"/>
-    <param name="tf_prefix" value=""/>
-    <param name="frame_id" value="imu"/>
-  </node>
-
-</launch>
-
diff --git a/package.xml b/package.xml
index 366fb2df2cb7108ce63b97969eb3107ba8a1ddab..220a222f1ec5077a53baeb45be0ab95c40ae41f4 100644
--- a/package.xml
+++ b/package.xml
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package>
-  <name>bno055_imu</name>
+  <name>iri_bno055_imu_driver</name>
   <version>0.0.0</version>
-  <description>The bno055_imu package</description>
+  <description>The iri_bno055_imu_driver package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
   <!-- Example:  -->
@@ -19,7 +19,7 @@
   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
   <!-- Optional attribute type can be: website, bugtracker, or repository -->
   <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/bno055_imu</url> -->
+  <!-- <url type="website">http://wiki.ros.org/iri_bno055_imu_driver</url> -->
 
 
   <!-- Author tags are optional, mutiple are allowed, one per tag -->
@@ -51,4 +51,4 @@
     <!-- Other tools can request additional information be placed here -->
 
   </export>
-</package>
\ No newline at end of file
+</package>
diff --git a/src/bno055_imu_ros_driver.cpp b/src/bno055_imu_ros_driver.cpp
index 07d0a50f7fcfafd4a27555b6b162653124c15aec..1b03935cb095e0ef26bdd2ac6071998b2982e6f9 100644
--- a/src/bno055_imu_ros_driver.cpp
+++ b/src/bno055_imu_ros_driver.cpp
@@ -11,6 +11,7 @@ Bno055ImuDriver::Bno055ImuDriver(void)
 bool Bno055ImuDriver::openDriver(void)
 {
   //setDriverId(driver string id);
+  this->lock();
   if(this->imu_device!=NULL)
   {
     delete this->imu_device;
@@ -19,6 +20,8 @@ bool Bno055ImuDriver::openDriver(void)
   try{
     this->imu_device=new CBNO055IMUDriver("BNO055_driver");
     this->imu_device->open(this->config_.serial_device);
+    ROS_INFO("IMU successfully opened");
+    this->unlock();
     return true;
   }catch(CException &e){
     if(this->imu_device!=NULL)
@@ -27,32 +30,38 @@ bool Bno055ImuDriver::openDriver(void)
       this->imu_device=NULL;
     }
     ROS_WARN_STREAM(e.what());
+    this->unlock();
     return false;
   }
 }
 
 bool Bno055ImuDriver::closeDriver(void)
 {
+  this->lock();
   if(this->imu_device!=NULL)
   {
     delete this->imu_device;
     this->imu_device=NULL;
   }
+  this->unlock();
   return true;
 }
 
 bool Bno055ImuDriver::startDriver(void)
 {
+  this->lock();
   if(this->config_.cal_filename.size()>0)
   {
     try{
       this->imu_device->set_operation_mode(config_mode);
       this->imu_device->load_calibration(this->config_.cal_filename);
-      this->imu_device->set_operation_mode(ndof_mode);
+      this->imu_device->set_operation_mode((op_mode_t)this->config_.mode);
       this->imu_device->set_data_rate(this->config_.update_rate);
+      this->unlock();
       return true;
     }catch(CException &e){
       ROS_WARN_STREAM(e.what());
+      this->unlock();
       return false;
     }
   }
@@ -61,10 +70,12 @@ bool Bno055ImuDriver::startDriver(void)
     try{
       this->imu_device->set_operation_mode(config_mode);
       this->imu_device->set_data_rate(this->config_.update_rate);
-      this->imu_device->set_operation_mode(ndof_mode);
+      this->imu_device->set_operation_mode((op_mode_t)this->config_.mode);
+      this->unlock();
       return true;
     }catch(CException &e){
       ROS_WARN_STREAM(e.what());
+      this->unlock();
       return false;
     }
   }
@@ -72,10 +83,17 @@ bool Bno055ImuDriver::startDriver(void)
 
 bool Bno055ImuDriver::stopDriver(void)
 {
+  this->lock();
   this->imu_device->set_operation_mode(config_mode);
+  this->unlock();
   return true;
 }
 
+op_mode_t Bno055ImuDriver::get_operation_mode(void)
+{
+  return (op_mode_t)this->config_.mode;
+}
+
 void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level)
 {
   this->lock();
@@ -84,14 +102,19 @@ void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level)
   // update driver with new_cfg data
   switch(this->getState())
   {
-    case Bno055ImuDriver::CLOSED:
-      this->frame_id=new_cfg.tf_prefix+"/"+new_cfg.frame_id;
+    case iri_base_driver::CLOSED:
       break;
 
-    case Bno055ImuDriver::OPENED:
+    case iri_base_driver::OPENED:
+      this->imu_device->set_data_rate(new_cfg.update_rate);
+      this->frame_id=new_cfg.tf_prefix+"/"+new_cfg.frame_id;
+      // change configuration
+      this->imu_device->set_accel_config(new_cfg.accel_range,new_cfg.accel_bandwidth,new_cfg.accel_pwr_mode);
+      this->imu_device->set_gyro_config(new_cfg.gyro_range,new_cfg.gyro_bandwidth,new_cfg.gyro_pwr_mode);
+      this->imu_device->set_mag_config(new_cfg.mag_rate,new_cfg.mag_op_mode,new_cfg.mag_pwr_mode);
       break;
 
-    case Bno055ImuDriver::RUNNING:
+    case iri_base_driver::RUNNING:
       break;
   }
 
@@ -141,6 +164,30 @@ std::vector<double> Bno055ImuDriver::get_gravity(void)
     return std::vector<double>();
 }
 
+std::vector<double> Bno055ImuDriver::get_raw_accelerometer(void)
+{
+  if(this->imu_device!=NULL)
+    return this->imu_device->get_raw_accelerometer();
+  else
+    return std::vector<double>();
+}
+
+std::vector<double> Bno055ImuDriver::get_raw_magnetometer(void)
+{
+  if(this->imu_device!=NULL)
+    return this->imu_device->get_raw_magnetometer();
+  else
+    return std::vector<double>();
+}
+
+std::vector<double> Bno055ImuDriver::get_raw_gyroscope(void)
+{
+  if(this->imu_device!=NULL)
+    return this->imu_device->get_raw_gyroscope();
+  else
+    return std::vector<double>();
+}
+
 bool Bno055ImuDriver::is_imu_calibrated(void)
 {
   bool accel_cal,mag_cal,gyro_cal;
@@ -159,11 +206,11 @@ bool Bno055ImuDriver::is_imu_calibrated(void)
       else
       {
         if(!accel_cal)
-          ROS_WARN("Accelerometer not calibrated: Move the sensor slowly to six different positions and hold it steady for a few seconds");
+          ROS_DEBUG("Accelerometer not calibrated: Move the sensor slowly to six different positions and hold it steady for a few seconds");
         if(!mag_cal)
-          ROS_WARN("Magnetometer no calibrated: Move the sensor randomly");
+          ROS_DEBUG("Magnetometer no calibrated: Move the sensor randomly");
         if(!gyro_cal)
-          ROS_WARN("Gyroscope not calibrated: Hold the sensor steady at any position for a few seconds");
+          ROS_DEBUG("Gyroscope not calibrated: Hold the sensor steady at any position for a few seconds");
         return false;
       }
     }catch(CException &e){
diff --git a/src/bno055_imu_ros_driver_node.cpp b/src/bno055_imu_ros_driver_node.cpp
index de25be4a5d380077dc33fd3d73ca4f0bbfa5bd82..47e23654133502d76e1acdca4d185936c568bca6 100644
--- a/src/bno055_imu_ros_driver_node.cpp
+++ b/src/bno055_imu_ros_driver_node.cpp
@@ -7,10 +7,11 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) :
   unsigned int i;
 
   //init class attributes if necessary
-  this->loop_rate_ = 500;//in [Hz]
+  this->setRate(500);//in [Hz]
 
   // [init publishers]
-  this->imu_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Imu>("imu", 1);
+  this->magnetometer_publisher_ = this->private_node_handle_.advertise<sensor_msgs::MagneticField>("magnetometer", 1);
+  this->imu_publisher_ = this->private_node_handle_.advertise<sensor_msgs::Imu>("imu", 1);
   
   // [init subscribers]
   
@@ -29,39 +30,67 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) :
     this->imu_Imu_msg_.orientation_covariance[i]=0.0;
     this->imu_Imu_msg_.angular_velocity_covariance[i]=0.0;
     this->imu_Imu_msg_.linear_acceleration_covariance[i]=0.0;
+    this->magnetometer_MagneticField_msg_.magnetic_field_covariance[i]=0.0;
   }
-  this->imu_Imu_msg_.angular_velocity_covariance[0]=-1.0;
 }
 
 void Bno055ImuDriverNode::mainNodeThread(void)
 {
-  std::vector<double> quaternion,linear_accel;
+  std::vector<double> quaternion,linear_accel,mag,gyro,accel;
+  op_mode_t op_mode;
 
   //lock access to driver if necessary
-  this->driver_.lock();
 
-  if(this->driver_.is_imu_calibrated())
+  this->driver_.lock();
+  if(this->driver_.getState()==iri_base_driver::RUNNING)
   {
+    this->driver_.is_imu_calibrated();
     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)
+      {
+        mag=this->driver_.get_raw_magnetometer();
+        this->magnetometer_MagneticField_msg_.header.stamp=ros::Time::now();
+        this->magnetometer_MagneticField_msg_.header.frame_id=this->driver_.get_frame_id();
+        this->magnetometer_MagneticField_msg_.magnetic_field.x=mag[0];
+        this->magnetometer_MagneticField_msg_.magnetic_field.y=mag[1];
+        this->magnetometer_MagneticField_msg_.magnetic_field.z=mag[2];
+        this->magnetometer_publisher_.publish(this->magnetometer_MagneticField_msg_);
+      }
       quaternion=this->driver_.get_orientation_quat();
       linear_accel=this->driver_.get_linear_accel();
+      accel=this->driver_.get_raw_accelerometer();
+      gyro=this->driver_.get_raw_gyroscope();
       this->imu_Imu_msg_.header.stamp=ros::Time::now();
       this->imu_Imu_msg_.header.frame_id=this->driver_.get_frame_id();
       this->imu_Imu_msg_.orientation.x=quaternion[0];
       this->imu_Imu_msg_.orientation.y=quaternion[1];
       this->imu_Imu_msg_.orientation.z=quaternion[2];
       this->imu_Imu_msg_.orientation.w=quaternion[3];
-      this->imu_Imu_msg_.linear_acceleration.x=linear_accel[0];
-      this->imu_Imu_msg_.linear_acceleration.y=linear_accel[1];
-      this->imu_Imu_msg_.linear_acceleration.z=linear_accel[2];
+      this->imu_Imu_msg_.angular_velocity.x=gyro[0];
+      this->imu_Imu_msg_.angular_velocity.y=gyro[1];
+      this->imu_Imu_msg_.angular_velocity.z=gyro[2];
+      if(op_mode>=imu_mode)
+      {
+        this->imu_Imu_msg_.linear_acceleration.x=linear_accel[0];
+        this->imu_Imu_msg_.linear_acceleration.y=linear_accel[1];
+        this->imu_Imu_msg_.linear_acceleration.z=linear_accel[2];
+      }
+      else
+      {
+        this->imu_Imu_msg_.linear_acceleration.x=accel[0];
+        this->imu_Imu_msg_.linear_acceleration.y=accel[1];
+        this->imu_Imu_msg_.linear_acceleration.z=accel[2];
+      }
       this->imu_publisher_.publish(this->imu_Imu_msg_);
     }catch(CException &e){  
       ROS_WARN_STREAM(e.what());
     }
   }
+  this->driver_.unlock();
 
   // [fill msg Header if necessary]
 
@@ -74,7 +103,6 @@ void Bno055ImuDriverNode::mainNodeThread(void)
   // [publish messages]
 
   //unlock access to driver if previously blocked
-  this->driver_.unlock();
 }
 
 /*  [subscriber callbacks] */
@@ -117,5 +145,5 @@ Bno055ImuDriverNode::~Bno055ImuDriverNode(void)
 /* main function */
 int main(int argc,char *argv[])
 {
-  return driver_base::main<Bno055ImuDriverNode>(argc, argv, "bno055_imu_driver_node");
+  return iri_base_driver::main<Bno055ImuDriverNode>(argc, argv, "bno055_imu_driver_node");
 }