sensor: sensor events timestamp out of expected range
fix sensor events timestamp out of expected range during CTS verifier test.
Change-Id: I271505dfb480374949d57b997d08d618dde0bcc2
Tracked-On: https://jira.ndg.intel.com/browse/MARVIN-914
Signed-off-by: Baixing Tan <baixingx.tan@intel.com>
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];
