diff --git a/src/driver.cpp b/src/driver.cpp
index c66a86f7822bf4642d3d1c3b7821065b80c5899e..0868620e05cf5332c6ddc2a5d29b64ca34209ee0 100644
--- a/src/driver.cpp
+++ b/src/driver.cpp
@@ -6,6 +6,19 @@
 #include <cmath>
 #include <limits>
 
+
+// TEST
+
+#include <ros/console.h>
+#include <iostream>
+#include <chrono>
+#include <thread>
+#include <functional>
+
+#define US_PERIOD_TIME_SCAN 1000
+
+
+
 // CONSTRUCTORS
 driver::driver()
 {
@@ -80,7 +93,39 @@ void driver::initialize(unsigned int i2c_bus, unsigned int i2c_address, unsigned
 
     // Power on magnetometer at 16bit resolution with 100Hz sample rate.
     write_ak8963_register(register_ak8963_type::CONTROL_1, 0x16);
+
+    // start force read_data every US_PERIOD_TIME_SCAN useconds.
+    std::thread([&]() {
+	    while (true)
+            {
+                driver::read_data();
+	        std::this_thread::sleep_for(std::chrono::microseconds(US_PERIOD_TIME_SCAN);
+	    }
+    }).detach();
+
+
 }
+
+/* removed to be more efficient
+int driver::get_sensors_availability(){
+
+    char int_status;
+    try
+    {
+	int_status = read_mpu9250_register(driver::register_mpu9250_type::INT_STATUS);
+    }
+    catch(const std::exception& e)
+    {
+	// Quit before callback. Do not report error in loop.
+	return 0;
+    }
+
+    int_status = int_status & (char) 0b00000001;
+
+    return (int) int_status;
+}
+*/
+
 void driver::deinitialize()
 {
     // Power down the AK8963.
@@ -121,8 +166,7 @@ float driver::p_dlpf_frequencies(gyro_dlpf_frequency_type gyro_frequency, accel_
     {
         gyro_dlpf_frequency = 5;
         gyro_internal_frequency = 1000;
-        break;
-    }
+        break; }
     case driver::gyro_dlpf_frequency_type::F_10Hz:
     {
         gyro_dlpf_frequency = 10;
@@ -365,6 +409,7 @@ void driver::read_data()
         data.magneto_z = 4900.0f * static_cast<float>(static_cast<short>(le16toh(*reinterpret_cast<unsigned short*>(&magneto_buffer[4])))) / resolution;
     }
 
+    /*
     // Read interrupt status register to clear interrupt.
     try
     {
@@ -376,6 +421,7 @@ void driver::read_data()
         return;
     }
 
+    */
     // Initiate the data callback.
     driver::m_data_callback(data);
 }