diff --git a/libsensors_iio/src/HWSensorBase.cpp b/libsensors_iio/src/HWSensorBase.cpp
index 43f7fbb..c129ed3 100644
--- a/libsensors_iio/src/HWSensorBase.cpp
+++ b/libsensors_iio/src/HWSensorBase.cpp
@@ -446,7 +446,7 @@
 	if (!GetStatusOfHandle(sensor_t_data.handle))
 		return;
 
-	if (sensor_event.timestamp >= (last_data_timestamp + real_pollrate)) {
+	if (sensor_event.timestamp >= last_data_timestamp) {
 		err = write(android_pipe_fd, &sensor_event, sizeof(sensor_event));
 		if (err < 0) {
 			ALOGE("%s: Failed to write sensor data to pipe.", android_name);
@@ -454,5 +454,6 @@
 		}
 
 		last_data_timestamp = sensor_event.timestamp;
-	}
+	} else
+		ALOGE("Timestamp out of order, event from type=%d dropped", sensor_event.type);
 }
diff --git a/libsensors_iio/src/SWAccelGyroFusion6X.cpp b/libsensors_iio/src/SWAccelGyroFusion6X.cpp
index 83f8bd5..95e02f7 100644
--- a/libsensors_iio/src/SWAccelGyroFusion6X.cpp
+++ b/libsensors_iio/src/SWAccelGyroFusion6X.cpp
@@ -112,29 +112,19 @@
 {
 	int64_t time_diff = 0;
 	SensorBaseData accel_data, gyro_data;
-	int err, data_remaining_gyro, nomaxdata = 10;
+	int err, data_remaining_accel, nomaxdata = 10;
 
+	memset(&gyro_data, 0, sizeof(SensorBaseData));
 	do {
-		data_remaining_gyro = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_1, &gyro_data);
-		if (data_remaining_gyro < 0)
-			return;
 
-		do {
-			err = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data);
-			if (err < 0) {
-				nomaxdata--;
-				usleep(200);
-				continue;
-			}
+		data_remaining_accel = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data);
+		if (data_remaining_accel < 0) {
+			return ;
+		}
 
-			time_diff = gyro_data.timestamp - accel_data.timestamp;
+		vSensor_API_Run_6X(accel_data.raw, gyro_data.processed, accel_data.timestamp);
 
-		} while ((time_diff >= GetRealPollrate()) && (nomaxdata > 0));
-
-		if (err >= 0)
-			vSensor_API_Run_6X(accel_data.raw, gyro_data.processed, gyro_data.timestamp);
-
-		sensor_event.timestamp = gyro_data.timestamp;
+		sensor_event.timestamp = accel_data.timestamp;
 
 		err = vSensor_API_Get_Quaternion_6X(outdata[ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID].processed);
 		if (err < 0)
@@ -153,5 +143,5 @@
 		outdata[ST_ACCEL_GYRO_GRAVITY_OUT_ID].timestamp = sensor_event.timestamp;
 
 		SplitAndProcessData(outdata);
-	} while (data_remaining_gyro > 0);
+	} while (data_remaining_accel > 0);
 }
diff --git a/libsensors_iio/src/SWSensorBase.cpp b/libsensors_iio/src/SWSensorBase.cpp
index 0acd26d..e4db3c6 100644
--- a/libsensors_iio/src/SWSensorBase.cpp
+++ b/libsensors_iio/src/SWSensorBase.cpp
@@ -126,6 +126,8 @@
 			temp_real_pollrate = GetMinPeriod();
 	}
 
+	last_data_timestamp = GetTimestamp();
+
 	return 0;
 }
 
@@ -136,13 +138,13 @@
 	if (!GetStatusOfHandle(sensor_t_data.handle))
 		return;
 
-	if (sensor_event.timestamp >= (last_data_timestamp + real_pollrate)) {
+	if (sensor_event.timestamp >= last_data_timestamp) {
 		err = write(android_pipe_fd, &sensor_event, sizeof(sensor_event));
 		if (err < 0) {
 			ALOGE("%s: Failed to write sensor data to pipe.", android_name);
 			return;
 		}
-
 		last_data_timestamp = sensor_event.timestamp;
-	}
+	} else
+		ALOGE("Timestamp out of order, event from type=%d dropped", sensor_event.type);
 }
diff --git a/libsensors_iio/src/SensorBase.cpp b/libsensors_iio/src/SensorBase.cpp
index c6cb908..b5d7e7f 100644
--- a/libsensors_iio/src/SensorBase.cpp
+++ b/libsensors_iio/src/SensorBase.cpp
@@ -105,6 +105,17 @@
 	return (char *)sensor_t_data.name;
 }
 
+int64_t SensorBase::GetTimestamp()
+{
+    struct timespec t;
+
+    t.tv_sec = 0;
+        t.tv_nsec = 0;
+    clock_gettime(CLOCK_BOOTTIME, &t);
+
+    return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;
+}
+
 int SensorBase::Enable(int handle, bool enable)
 {
 	int err, i = 0;
@@ -129,6 +140,7 @@
 
 		ResetBitEnableMask(handle);
 	}
+	last_data_timestamp = GetTimestamp();
 
 #if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_INFO)
 	if (((old_status && !GetStatus()) || (!old_status && GetStatus())) && (sensor_t_data.type < SENSOR_TYPE_ST_CUSTOM_NO_SENSOR)) {
diff --git a/libsensors_iio/src/SensorBase.h b/libsensors_iio/src/SensorBase.h
index 9b73dcc..dc425ba 100644
--- a/libsensors_iio/src/SensorBase.h
+++ b/libsensors_iio/src/SensorBase.h
@@ -123,6 +123,8 @@
 
 	char* GetName();
 
+	int64_t GetTimestamp();
+
 	virtual int Enable(int handle, bool enable);
 	bool GetStatus();
 
diff --git a/libsensors_iio/src/SensorHAL.cpp b/libsensors_iio/src/SensorHAL.cpp
index 90f1931..22213d3 100644
--- a/libsensors_iio/src/SensorHAL.cpp
+++ b/libsensors_iio/src/SensorHAL.cpp
@@ -705,7 +705,6 @@
 		return err;
 
 	fcntl(pipe_fd[0], F_SETFL, O_NONBLOCK);
-	fcntl(pipe_fd[1], F_SETFL, O_NONBLOCK);
 
 	hal_data->android_pollfd.events = POLLIN;
 	hal_data->android_pollfd.fd = pipe_fd[0];