From c4306755911e922ee72de7e8f159368538f0d94f Mon Sep 17 00:00:00 2001 From: Saadi Maalem Date: Fri, 16 Oct 2015 21:12:53 +0200 Subject: 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 --- libsensors_iio/src/HWSensorBase.cpp | 5 +++-- libsensors_iio/src/SWAccelGyroFusion6X.cpp | 28 +++++++++------------------- libsensors_iio/src/SWSensorBase.cpp | 8 +++++--- libsensors_iio/src/SensorBase.cpp | 12 ++++++++++++ libsensors_iio/src/SensorBase.h | 2 ++ libsensors_iio/src/SensorHAL.cpp | 1 - 6 files changed, 31 insertions(+), 25 deletions(-) (limited to 'libsensors_iio') 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 @@ void HWSensorBaseWithPollrate::WriteDataToPipe() 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 @@ 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 83f8bd5..95e02f7 100644 --- a/libsensors_iio/src/SWAccelGyroFusion6X.cpp +++ b/libsensors_iio/src/SWAccelGyroFusion6X.cpp @@ -112,29 +112,19 @@ void SWAccelGyroFusion6X::TriggerEventReceived() { 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; - } - - time_diff = gyro_data.timestamp - accel_data.timestamp; - } while ((time_diff >= GetRealPollrate()) && (nomaxdata > 0)); + data_remaining_accel = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data); + if (data_remaining_accel < 0) { + return ; + } - if (err >= 0) - vSensor_API_Run_6X(accel_data.raw, gyro_data.processed, gyro_data.timestamp); + vSensor_API_Run_6X(accel_data.raw, gyro_data.processed, accel_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 @@ void SWAccelGyroFusion6X::TriggerEventReceived() 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 @@ int SWSensorBaseWithPollrate::SetDelay(int handle, int64_t period_ns, int64_t ti temp_real_pollrate = GetMinPeriod(); } + last_data_timestamp = GetTimestamp(); + return 0; } @@ -136,13 +138,13 @@ void SWSensorBaseWithPollrate::WriteDataToPipe() 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 @@ 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; @@ -129,6 +140,7 @@ 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 9b73dcc..dc425ba 100644 --- a/libsensors_iio/src/SensorBase.h +++ b/libsensors_iio/src/SensorBase.h @@ -123,6 +123,8 @@ 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 90f1931..22213d3 100644 --- a/libsensors_iio/src/SensorHAL.cpp +++ b/libsensors_iio/src/SensorHAL.cpp @@ -705,7 +705,6 @@ 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]; -- cgit v1.2.3