Revert "sensor: sensor events timestamp out of expected range"android-n-preview-1

This reverts commit c4306755911e922ee72de7e8f159368538f0d94f.

Change-Id: I2efae19490b5cb7e0669ebda75a6f84e29a35230

6 files changed, 25 insertions, 31 deletions

diff --git a/libsensors_iio/src/HWSensorBase.cpp b/libsensors_iio/src/HWSensorBase.cpp
index c129ed3..43f7fbb 100644
--- a/libsensors_iio/src/HWSensorBase.cpp
+++ b/libsensors_iio/src/HWSensorBase.cpp
@@ -446,7 +446,7 @@ void HWSensorBaseWithPollrate::WriteDataToPipe()
 	if (!GetStatusOfHandle(sensor_t_data.handle))
 		return;
 
-	if (sensor_event.timestamp >= last_data_timestamp) {
+	if (sensor_event.timestamp >= (last_data_timestamp + real_pollrate)) {
 		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,6 +454,5 @@ void HWSensorBaseWithPollrate::WriteDataToPipe()
 		}
 
 		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 95e02f7..83f8bd5 100644
--- a/libsensors_iio/src/SWAccelGyroFusion6X.cpp
+++ b/libsensors_iio/src/SWAccelGyroFusion6X.cpp
@@ -112,19 +112,29 @@ void SWAccelGyroFusion6X::TriggerEventReceived()
 {
 	int64_t time_diff = 0;
 	SensorBaseData accel_data, gyro_data;
-	int err, data_remaining_accel, nomaxdata = 10;
+	int err, data_remaining_gyro, 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;
 
-		data_remaining_accel = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data);
-		if (data_remaining_accel < 0) {
-			return ;
-		}
+		do {
+			err = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data);
+			if (err < 0) {
+				nomaxdata--;
+				usleep(200);
+				continue;
+			}
+
+			time_diff = gyro_data.timestamp - accel_data.timestamp;
+
+		} while ((time_diff >= GetRealPollrate()) && (nomaxdata > 0));
 
-		vSensor_API_Run_6X(accel_data.raw, gyro_data.processed, accel_data.timestamp);
+		if (err >= 0)
+			vSensor_API_Run_6X(accel_data.raw, gyro_data.processed, gyro_data.timestamp);
 
-		sensor_event.timestamp = accel_data.timestamp;
+		sensor_event.timestamp = gyro_data.timestamp;
 
 		err = vSensor_API_Get_Quaternion_6X(outdata[ST_ACCEL_GYRO_ROTATION_VECTOR_OUT_ID].processed);
 		if (err < 0)
@@ -143,5 +153,5 @@ void SWAccelGyroFusion6X::TriggerEventReceived()
 		outdata[ST_ACCEL_GYRO_GRAVITY_OUT_ID].timestamp = sensor_event.timestamp;
 
 		SplitAndProcessData(outdata);
-	} while (data_remaining_accel > 0);
+	} while (data_remaining_gyro > 0);
 }
diff --git a/libsensors_iio/src/SWSensorBase.cpp b/libsensors_iio/src/SWSensorBase.cpp
index e4db3c6..0acd26d 100644
--- a/libsensors_iio/src/SWSensorBase.cpp
+++ b/libsensors_iio/src/SWSensorBase.cpp
@@ -126,8 +126,6 @@ int SWSensorBaseWithPollrate::SetDelay(int handle, int64_t period_ns, int64_t ti
 			temp_real_pollrate = GetMinPeriod();
 	}
 
-	last_data_timestamp = GetTimestamp();
-
 	return 0;
 }
 
@@ -138,13 +136,13 @@ void SWSensorBaseWithPollrate::WriteDataToPipe()
 	if (!GetStatusOfHandle(sensor_t_data.handle))
 		return;
 
-	if (sensor_event.timestamp >= last_data_timestamp) {
+	if (sensor_event.timestamp >= (last_data_timestamp + real_pollrate)) {
 		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 b5d7e7f..c6cb908 100644
--- a/libsensors_iio/src/SensorBase.cpp
+++ b/libsensors_iio/src/SensorBase.cpp
@@ -105,17 +105,6 @@ char* SensorBase::GetName()
 	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;
@@ -140,7 +129,6 @@ int SensorBase::Enable(int handle, bool enable)
 
 		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 dc425ba..9b73dcc 100644
--- a/libsensors_iio/src/SensorBase.h
+++ b/libsensors_iio/src/SensorBase.h
@@ -123,8 +123,6 @@ public:
 
 	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 22213d3..90f1931 100644
--- a/libsensors_iio/src/SensorHAL.cpp
+++ b/libsensors_iio/src/SensorHAL.cpp
@@ -705,6 +705,7 @@ static int st_hal_create_android_pipe(STSensorHAL_data *hal_data)
 		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];