Sensors: Invensense: 6515: Merge timestamp fixes
Merged in timestamp related changes (rev 15145) from Invensense to
address CTS Verifier test failures related to sensor event timestamps.
Bug: 25290258
Change-Id: I6af3a04bd3de5e2f9a4dcbc5ad81f22b45d90940
diff --git a/6515/libsensors_iio/Android.mk b/6515/libsensors_iio/Android.mk
index f33c7ae..f894e50 100755
--- a/6515/libsensors_iio/Android.mk
+++ b/6515/libsensors_iio/Android.mk
@@ -30,6 +30,19 @@
MINOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f2 -d.)
VERSION_KK :=$(shell test $(MAJOR_VERSION) -eq 4 -a $(MINOR_VERSION) -gt 3 && echo true)
VERSION_L :=$(shell test $(MAJOR_VERSION) -eq 5 -a $(MINOR_VERSION) -eq 0 && echo true)
+
+#
+# Invensense uses the OS version to determine whether to include batch support,
+# but implemented it in a way that requires modifying the code each time we move
+# to a newer OS version. I will fix this problem in a subsequent change, but for now,
+# hardcode to saying we're ANDROID_L so we can isolate this checkin to being
+# only changes coming from Invensense.
+#
+# Setting ANDROID_L to true is perfectly safe even on ANDROID_M because the code
+# just requires "ANDROID_L or newer"
+#
+VERSION_L :=true
+
$(info YD>>ANDRIOD VERSION=$(MAJOR_VERSION).$(MINOR_VERSION))
$(info YD>>VERSION_L=$(VERSION_L), VERSION_KK=$(VERSION_KK))
#ANDROID version check END
diff --git a/6515/libsensors_iio/MPLSensor.cpp b/6515/libsensors_iio/MPLSensor.cpp
index 0ef2c95..c7d1e6e 100644
--- a/6515/libsensors_iio/MPLSensor.cpp
+++ b/6515/libsensors_iio/MPLSensor.cpp
@@ -37,6 +37,7 @@
#include <string.h>
#include <linux/input.h>
#include <utils/Atomic.h>
+#include <utils/SystemClock.h>
#include "MPLSensor.h"
#include "PressureSensor.IIO.secondary.h"
@@ -75,7 +76,7 @@
* MPLSensor class implementation
******************************************************************************/
-static struct timespec mt_pre;
+static int64_t mt_pre_ns;
// following extended initializer list would only be available with -std=c++11
// or -std=gnu+11
@@ -97,6 +98,7 @@
mDmpPedometerEnabled(0),
mDmpStepCountEnabled(0),
mEnabled(0),
+ mEnabledCached(0),
mBatchEnabled(0),
mOldBatchEnabledMask(0),
mAccelInputReader(4),
@@ -142,6 +144,7 @@
mLeftOverBufferSize(0),
mInitial6QuatValueAvailable(0),
mSkipReadEvents(0),
+ mSkipExecuteOnData(0),
mDataMarkerDetected(0),
mEmptyDataMarkerDetected(0) {
VFUNC_LOG;
@@ -165,6 +168,7 @@
memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
memset(mInitial6QuatValue, 0, sizeof(mInitial6QuatValue));
mFlushSensorEnabledVector.setCapacity(NumSensors);
+ memset(mEnabledTime, 0, sizeof(mEnabledTime));
/* setup sysfs paths */
inv_init_sysfs_attributes();
@@ -1094,7 +1098,7 @@
int res = 0;
// set sensor engine and fifo
- if ((mFeatureActiveMask & DMP_FEATURE_MASK) || en) {
+ if (((mFeatureActiveMask & ~INV_DMP_BATCH_MODE) & DMP_FEATURE_MASK) || en) {
if ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) ||
(mFeatureActiveMask & INV_DMP_PED_QUATERNION) ||
(mFeatureActiveMask & INV_DMP_QUATERNION)) {
@@ -1821,7 +1825,7 @@
mFeatureActiveMask |= INV_DMP_PEDOMETER_STEP;
}
- clock_gettime(CLOCK_BOOTTIME, &mt_pre);
+ mt_pre_ns = android::elapsedRealtimeNano();
} else {
if (interruptMode) {
mFeatureActiveMask &= ~INV_DMP_PEDOMETER;
@@ -2612,6 +2616,12 @@
update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status,
&s->timestamp);
#endif
+ if (!mEnabledTime[Gyro] || !(s->timestamp > mEnabledTime[Gyro])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:gyro incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[Gyro], s->timestamp, android::elapsedRealtimeNano());
+ update = 0;
+ }
+
LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
return update;
@@ -2628,6 +2638,12 @@
update = inv_get_sensor_type_gyroscope_raw(s->uncalibrated_gyro.uncalib,
&s->gyro.status, &s->timestamp);
#endif
+ if (!mEnabledTime[RawGyro] || !(s->timestamp > mEnabledTime[RawGyro])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:raw gyro incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[RawGyro], s->timestamp, android::elapsedRealtimeNano());
+ update = 0;
+ }
+
if(update) {
memcpy(s->uncalibrated_gyro.bias, mGyroBias, sizeof(mGyroBias));
LOGV_IF(HANDLER_DATA,"HAL:gyro bias data : %+f %+f %+f -- %lld - %d",
@@ -2652,6 +2668,12 @@
update = inv_get_sensor_type_accelerometer(
s->acceleration.v, &s->acceleration.status, &s->timestamp);
#endif
+ if (!mEnabledTime[Accelerometer] || !(s->timestamp > mEnabledTime[Accelerometer])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:accel incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[Accelerometer], s->timestamp, android::elapsedRealtimeNano());
+ update = 0;
+ }
+
LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
s->timestamp, update);
@@ -2663,6 +2685,7 @@
{
VHANDLER_LOG;
int update;
+ int overflow = mCompassOverFlow;
#if defined ANDROID_LOLLIPOP
update = inv_get_sensor_type_magnetic_field(
s->magnetic.v, &s->magnetic.status, (inv_time_t *)(&s->timestamp));
@@ -2670,17 +2693,24 @@
update = inv_get_sensor_type_magnetic_field(
s->magnetic.v, &s->magnetic.status, &s->timestamp);
#endif
+ if (!mEnabledTime[MagneticField] || !(s->timestamp > mEnabledTime[MagneticField])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:compass incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[MagneticField], s->timestamp, android::elapsedRealtimeNano());
+ overflow = 0;
+ update = 0;
+ }
LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2],
s->timestamp, update);
mCompassAccuracy = s->magnetic.status;
- return update | mCompassOverFlow;
+ return update | overflow;
}
int MPLSensor::rawCompassHandler(sensors_event_t* s)
{
VHANDLER_LOG;
int update;
+ int overflow = mCompassOverFlow;
//TODO: need to handle uncalib data and bias for 3rd party compass
#if defined ANDROID_LOLLIPOP
if(mCompassSensor->providesCalibration()) {
@@ -2699,6 +2729,12 @@
&s->magnetic.status, &s->timestamp);
}
#endif
+ if (!mEnabledTime[RawMagneticField] || !(s->timestamp > mEnabledTime[RawMagneticField])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:raw compass incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[RawMagneticField], s->timestamp, android::elapsedRealtimeNano());
+ overflow = 0;
+ update = 0;
+ }
if(update) {
memcpy(s->uncalibrated_magnetic.bias, mCompassBias, sizeof(mCompassBias));
LOGV_IF(HANDLER_DATA, "HAL:compass bias data: %+f %+f %+f -- %lld - %d",
@@ -2709,7 +2745,7 @@
LOGV_IF(HANDLER_DATA, "HAL:compass raw data: %+f %+f %+f %d -- %lld - %d",
s->uncalibrated_magnetic.uncalib[0], s->uncalibrated_magnetic.uncalib[1],
s->uncalibrated_magnetic.uncalib[2], s->magnetic.status, s->timestamp, update);
- return update | mCompassOverFlow;
+ return update | overflow;
}
/*
@@ -2729,7 +2765,15 @@
&s->timestamp);
#endif
s->orientation.status = status;
+
update |= isCompassDisabled();
+
+ if (!mEnabledTime[RotationVector] || !(s->timestamp > mEnabledTime[RotationVector])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:rv incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[RotationVector], s->timestamp, android::elapsedRealtimeNano());
+ update = 0;
+ }
+
LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f %+f %d- %+lld - %d",
s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->orientation.status, s->timestamp,
update);
@@ -2755,6 +2799,12 @@
#endif
s->orientation.status = status;
+ if (!mEnabledTime[GameRotationVector] || !(s->timestamp > mEnabledTime[GameRotationVector])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:grv incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[GameRotationVector], s->timestamp, android::elapsedRealtimeNano());
+ update = 0;
+ }
+
LOGV_IF(HANDLER_DATA, "HAL:grv data: %+f %+f %+f %+f %+f %d- %+lld - %d",
s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->orientation.status, s->timestamp,
update);
@@ -2773,6 +2823,13 @@
s->gyro.v, &s->gyro.status, &s->timestamp);
#endif
update |= isCompassDisabled();
+
+ if (!mEnabledTime[LinearAccel] || !(s->timestamp > mEnabledTime[LinearAccel])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:la incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[LinearAccel], s->timestamp, android::elapsedRealtimeNano());
+ update = 0;
+ }
+
LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
return update;
@@ -2790,6 +2847,13 @@
&s->timestamp);
#endif
update |= isCompassDisabled();
+
+ if (!mEnabledTime[Gravity] || !(s->timestamp > mEnabledTime[Gravity])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:gr incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[Gravity], s->timestamp, android::elapsedRealtimeNano());
+ update = 0;
+ }
+
LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
return update;
@@ -2808,6 +2872,13 @@
#endif
update |= isCompassDisabled();
+
+ if (!mEnabledTime[Orientation] || !(s->timestamp > mEnabledTime[Orientation])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:or incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[Orientation], s->timestamp, android::elapsedRealtimeNano());
+ update = 0;
+ }
+
LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
s->orientation.v[0], s->orientation.v[1], s->orientation.v[2],
s->timestamp, update);
@@ -2825,9 +2896,13 @@
s->data[2] = 0.f;
/* Capture timestamp in HAL */
- struct timespec ts;
- clock_gettime(CLOCK_BOOTTIME, &ts);
- s->timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+ s->timestamp = android::elapsedRealtimeNano();
+
+ if (!mEnabledTime[SignificantMotion] || !(s->timestamp > mEnabledTime[SignificantMotion])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:sm incorrect timestamp Enabled=%lld, Timestamp=%lld",
+ mEnabledTime[SignificantMotion], s->timestamp);
+ update = 0;
+ }
LOGV_IF(HANDLER_DATA, "HAL:sm data: %f - %lld - %d",
s->data[0], s->timestamp, update);
@@ -2839,6 +2914,7 @@
VHANDLER_LOG;
int8_t status;
int update = 0;
+
#if defined ANDROID_LOLLIPOP
update = inv_get_sensor_type_geomagnetic_rotation_vector(s->data, &status,
(inv_time_t *)(&s->timestamp));
@@ -2847,6 +2923,13 @@
&s->timestamp);
#endif
s->orientation.status = status;
+
+ if (!mEnabledTime[GeomagneticRotationVector] || !(s->timestamp > mEnabledTime[GeomagneticRotationVector])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:gm incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[GeomagneticRotationVector], s->timestamp, android::elapsedRealtimeNano());
+ update = 0;
+ }
+
LOGV_IF(HANDLER_DATA, "HAL:gm data: %+f %+f %+f %+f %+f %d- %+lld - %d",
s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->orientation.status, s->timestamp, update);
return update < 1 ? 0 :1;
@@ -2867,6 +2950,14 @@
update = mPressureUpdate;
mPressureUpdate = 0;
+#ifdef ENABLE_PRESSURE
+ if (!mEnabledTime[Pressure] || !(s->timestamp > mEnabledTime[Pressure])) {
+ LOGV_IF(ENG_VERBOSE, "HAL:ps incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
+ mEnabledTime[Pressure], s->timestamp, android::elapsedRealtimeNano());
+ update = 0;
+ }
+#endif
+
LOGV_IF(HANDLER_DATA, "HAL:ps data: %+f %+f %+f %+f- %+lld - %d",
s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
return update < 1 ? 0 :1;
@@ -2884,9 +2975,7 @@
s->data[2] = 0.f;
/* get current timestamp */
- struct timespec ts;
- clock_gettime(CLOCK_BOOTTIME, &ts) ;
- s->timestamp = (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec;
+ s->timestamp = android::elapsedRealtimeNano();
LOGV_IF(HANDLER_DATA, "HAL:sd data: %f - %lld - %d",
s->data[0], s->timestamp, update);
@@ -2910,14 +2999,7 @@
#endif
if (s->timestamp == 0 && update) {
- struct timespec ts;
- clock_gettime(CLOCK_BOOTTIME, &ts);
- s->timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
- // workaround for some platform which has gap between monotonic clock
- // and Android SystemClock.
- // Subtract 100ms not to point the future for SystemClock.
- // s->timestamp -= 100000000LL;
- LOGV_IF(0, "HAL:sc timestamp %lld", s->timestamp);
+ s->timestamp = android::elapsedRealtimeNano();
}
return update;
@@ -2974,7 +3056,7 @@
if (!en)
mBatchEnabled &= ~(1 << handle);
- LOGV_IF(ENG_VERBOSE, "HAL:handle = %d", handle);
+ LOGV_IF(ENG_VERBOSE, "HAL: MPLSensor::enable(handle = %d, en = %d)", handle, en);
switch (handle) {
case ID_SC:
@@ -2986,8 +3068,16 @@
(en? "en" : "dis"));
enableDmpPedometer(en, 0);
mDmpStepCountEnabled = !!en;
+ if (en)
+ mEnabledTime[StepCounter] = android::elapsedRealtimeNano();
+ else
+ mEnabledTime[StepCounter] = 0;
+
+ if (!en)
+ mBatchDelays[what] = 1000000000LL;
return 0;
case ID_P:
+ what = StepDetector;
sname = "StepDetector";
LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
sname.string(), handle,
@@ -3001,6 +3091,13 @@
setBatch(batchMode,1);
}
mOldBatchEnabledMask = batchMode;
+ if (en)
+ mEnabledTime[StepDetector] = android::elapsedRealtimeNano();
+ else
+ mEnabledTime[StepDetector] = 0;
+
+ if (!en)
+ mBatchDelays[what] = 1000000000LL;
return 0;
case ID_SM:
sname = "Significant Motion";
@@ -3010,6 +3107,10 @@
(en? "en" : "dis"));
enableDmpSignificantMotion(en);
mDmpSignificantMotionEnabled = !!en;
+ if (en)
+ mEnabledTime[SignificantMotion] = android::elapsedRealtimeNano();
+ else
+ mEnabledTime[SignificantMotion] = 0;
return 0;
case ID_SO:
sname = "Screen Orientation";
@@ -3096,6 +3197,11 @@
mEnabled &= ~(1 << what);
mEnabled |= (uint32_t(flags) << what);
+ if (lastEnabled > mEnabled) {
+ mEnabledCached = mEnabled;
+ } else {
+ mEnabledCached = lastEnabled;
+ }
LOGV_IF(ENG_VERBOSE, "HAL:flags = %d", flags);
computeLocalSensorMask(mEnabled);
@@ -3186,6 +3292,16 @@
}
LOGV_IF(ENG_VERBOSE, "HAL:changed = %d", changed);
enableSensors(sen_mask, flags, changed);
+ // update mEnabledCached afer all configurations done
+ mEnabledCached = mEnabled;
+
+ if (en)
+ mEnabledTime[what] = android::elapsedRealtimeNano();
+ else
+ mEnabledTime[what] = 0;
+
+ if (!en)
+ mBatchDelays[what] = 1000000000LL;
}
// pthread_mutex_unlock(&mMplMutex);
@@ -3716,7 +3832,8 @@
{
VHANDLER_LOG;
- inv_execute_on_data();
+ if (!mSkipExecuteOnData)
+ inv_execute_on_data();
int numEventReceived = 0;
@@ -3783,7 +3900,7 @@
}
// load up virtual sensors
- if (mEnabled & (1 << i)) {
+ if (mEnabledCached & (1 << i)) {
update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
mPendingMask |= (1 << i);
@@ -3804,18 +3921,13 @@
// handle flush complete event
for(int k = 0; k < flush_vec_size; k++) {
int sendEvent = metaHandler(&mPendingFlushEvents[k], META_DATA_FLUSH_COMPLETE);
- if(sendEvent && count > 0) {
+ if (sendEvent && count > 0) {
*data++ = mPendingFlushEvents[k];
count--;
numEventReceived++;
}
}
- if (!mEmptyDataMarkerDetected) {
- // turn off sensors in data_builder
- resetMplStates();
- }
-
// Double check flush status
if (mFlushSensorEnabledVector.isEmpty()) {
mEmptyDataMarkerDetected = 0;
@@ -3854,6 +3966,19 @@
char *rdataP = NULL;
bool doneFlag = 0;
+ /* flush buffer when no sensors are enabled */
+ if (mEnabledCached == 0 && mBatchEnabled == 0 && mDmpPedometerEnabled == 0) {
+ rsize = read(iio_fd, rdata, MAX_SUSPEND_BATCH_PACKET_SIZE);
+ if(rsize > 0) {
+ LOGV_IF(ENG_VERBOSE, "HAL:input data flush rsize=%d", (int)rsize);
+ }
+ mLeftOverBufferSize = 0;
+ mFlushBatchSet = 1;
+ mDataMarkerDetected = 0;
+ mEmptyDataMarkerDetected = 0;
+ return;
+ }
+
lp_quaternion_on = isLowPowerQuatEnabled() && checkLPQuaternion();
sixAxis_quaternion_on = check6AxisQuatEnabled();
ped_quaternion_on = checkPedQuatEnabled();
@@ -3954,6 +4079,7 @@
mFlushBatchSet = 1;
}
mEmptyDataMarkerDetected = 1;
+ mDataMarkerDetected = 1;
}
}
@@ -3985,6 +4111,7 @@
sensors, lp_quaternion_on, sixAxis_quaternion_on, ped_quaternion_on,
ped_standalone_on);
+ mSkipExecuteOnData = 1;
while (readCounter > 0) {
// since copied buffer is already accounted for, reset left over size
mLeftOverBufferSize = 0;
@@ -4021,7 +4148,6 @@
mFlushBatchSet = 1;
}
mDataMarkerDetected = 1;
- mSkipReadEvents = 1;
}
else if (data_format == DATA_FORMAT_EMPTY_MARKER) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "EMPTY MARKER DETECTED:0x%x", data_format);
@@ -4030,7 +4156,7 @@
mFlushBatchSet = 1;
}
mEmptyDataMarkerDetected = 1;
- mSkipReadEvents = 1;
+ mDataMarkerDetected = 1;
}
else if (data_format == DATA_FORMAT_QUAT) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "QUAT DETECTED:0x%x", data_format);
@@ -4239,6 +4365,7 @@
"HAL:input inv_read_temperature = %lld, timestamp= %lld",
temperature[0], temperature[1]);
inv_build_temp(temperature[0], temperature[1]);
+ mSkipExecuteOnData = 0;
}
#ifdef TESTING
long bias[3], temp, temp_slope[3];
@@ -4266,30 +4393,30 @@
"HAL:input inv_build_gyro: %+8d %+8d %+8d - %lld",
mCachedGyroData[0], mCachedGyroData[1],
mCachedGyroData[2], mGyroSensorTimestamp);
+ mSkipExecuteOnData = 0;
latestTimestamp = mGyroSensorTimestamp;
}
if (mask == DATA_FORMAT_ACCEL) {
- if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
- mPendingMask |= 1 << Accelerometer;
- inv_build_accel(mCachedAccelData, 0, mAccelSensorTimestamp);
- LOGV_IF(INPUT_DATA,
- "HAL:input inv_build_accel: %+8ld %+8ld %+8ld - %lld",
- mCachedAccelData[0], mCachedAccelData[1],
- mCachedAccelData[2], mAccelSensorTimestamp);
- /* remember inital 6 axis quaternion */
- inv_time_t tempTimestamp;
- inv_get_6axis_quaternion(mInitial6QuatValue, &tempTimestamp);
- if (mInitial6QuatValue[0] != 0 && mInitial6QuatValue[1] != 0 &&
- mInitial6QuatValue[2] != 0 && mInitial6QuatValue[3] != 0) {
- mInitial6QuatValueAvailable = 1;
- LOGV_IF(INPUT_DATA && ENG_VERBOSE,
- "HAL:input build 6q init: %+8ld %+8ld %+8ld %+8ld",
- mInitial6QuatValue[0], mInitial6QuatValue[1],
- mInitial6QuatValue[2], mInitial6QuatValue[3]);
- }
- latestTimestamp = mAccelSensorTimestamp;
+ mPendingMask |= 1 << Accelerometer;
+ inv_build_accel(mCachedAccelData, 0, mAccelSensorTimestamp);
+ LOGV_IF(INPUT_DATA,
+ "HAL:input inv_build_accel: %+8ld %+8ld %+8ld - %lld",
+ mCachedAccelData[0], mCachedAccelData[1],
+ mCachedAccelData[2], mAccelSensorTimestamp);
+ mSkipExecuteOnData = 0;
+ /* remember inital 6 axis quaternion */
+ inv_time_t tempTimestamp;
+ inv_get_6axis_quaternion(mInitial6QuatValue, &tempTimestamp);
+ if (mInitial6QuatValue[0] != 0 && mInitial6QuatValue[1] != 0 &&
+ mInitial6QuatValue[2] != 0 && mInitial6QuatValue[3] != 0) {
+ mInitial6QuatValueAvailable = 1;
+ LOGV_IF(INPUT_DATA && ENG_VERBOSE,
+ "HAL:input build 6q init: %+8ld %+8ld %+8ld %+8ld",
+ mInitial6QuatValue[0], mInitial6QuatValue[1],
+ mInitial6QuatValue[2], mInitial6QuatValue[3]);
}
+ latestTimestamp = mAccelSensorTimestamp;
}
if (mask == DATA_FORMAT_COMPASS_OF) {
@@ -4302,6 +4429,7 @@
"HAL:input inv_build_compass_of: %+8ld %+8ld %+8ld - %lld",
mCachedCompassData[0], mCachedCompassData[1],
mCachedCompassData[2], mCompassTimestamp);
+ mSkipExecuteOnData = 0;
resetCompass();
}
@@ -4317,6 +4445,7 @@
"HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
mCachedCompassData[0], mCachedCompassData[1],
mCachedCompassData[2], mCompassTimestamp);
+ mSkipExecuteOnData = 0;
latestTimestamp = mCompassTimestamp;
}
@@ -4337,6 +4466,7 @@
mCachedQuaternionData[0], mCachedQuaternionData[1],
mCachedQuaternionData[2],
mQuatSensorTimestamp);
+ mSkipExecuteOnData = 0;
latestTimestamp = mQuatSensorTimestamp;
}
@@ -4356,6 +4486,7 @@
"HAL:input inv_build_quat-6x: %+8ld %+8ld %+8ld - %lld",
mCached6AxisQuaternionData[0], mCached6AxisQuaternionData[1],
mCached6AxisQuaternionData[2], mQuatSensorTimestamp);
+ mSkipExecuteOnData = 0;
latestTimestamp = mQuatSensorTimestamp;
}
@@ -4375,27 +4506,26 @@
inv_build_quat(mCachedPedQuaternionData,
status,
mQuatSensorTimestamp);
-
LOGV_IF(INPUT_DATA,
"HAL:HAL:input inv_build_quat-ped_6x: %+8ld %+8ld %+8ld - %lld",
mCachedPedQuaternionData[0], mCachedPedQuaternionData[1],
mCachedPedQuaternionData[2], mQuatSensorTimestamp);
+ mSkipExecuteOnData = 0;
latestTimestamp = mQuatSensorTimestamp;
}
#ifdef ENABLE_PRESSURE
if ((mask ==DATA_FORMAT_PRESSURE) && mPressureSensor->isIntegrated()) {
int status = 0;
- if (mLocalSensorMask & INV_ONE_AXIS_PRESSURE) {
- latestTimestamp = mPressureTimestamp;
- mPressureUpdate = 1;
- inv_build_pressure(mCachedPressureData,
- status,
- mPressureTimestamp);
- LOGV_IF(INPUT_DATA,
- "HAL:input inv_build_pressure: %+8ld - %lld",
- mCachedPressureData, mPressureTimestamp);
- }
+ latestTimestamp = mPressureTimestamp;
+ mPressureUpdate = 1;
+ inv_build_pressure(mCachedPressureData,
+ status,
+ mPressureTimestamp);
+ LOGV_IF(INPUT_DATA,
+ "HAL:input inv_build_pressure: %+8ld - %lld",
+ mCachedPressureData, mPressureTimestamp);
+ mSkipExecuteOnData = 0;
}
#endif
/* take the latest timestamp */
@@ -4464,15 +4594,14 @@
status = mCompassSensor->getAccuracy();
status |= INV_CALIBRATED;
}
- if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
- inv_build_compass(mCachedCompassData, status,
- mCompassTimestamp);
- LOGV_IF(INPUT_DATA,
- "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
- mCachedCompassData[0], mCachedCompassData[1],
- mCachedCompassData[2], mCompassTimestamp);
- mSkipReadEvents = 0;
- }
+ inv_build_compass(mCachedCompassData, status,
+ mCompassTimestamp);
+ LOGV_IF(INPUT_DATA,
+ "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+ mCachedCompassData[0], mCachedCompassData[1],
+ mCachedCompassData[2], mCompassTimestamp);
+ mSkipReadEvents = 0;
+ mSkipExecuteOnData = 0;
}
// pthread_mutex_unlock(&mMplMutex);
@@ -4693,9 +4822,7 @@
temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
temp.screen_orientation = screen_orientation;
#endif
- struct timespec ts;
- clock_gettime(CLOCK_BOOTTIME, &ts);
- temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+ temp.timestamp = android::elapsedRealtimeNano();
*data++ = temp;
count--;
@@ -4763,12 +4890,11 @@
{
VFUNC_LOG;
if (mDmpStepCountEnabled) {
- struct timespec t_now;
+ int64_t t_now_ns;
int64_t interval = 0;
- clock_gettime(CLOCK_BOOTTIME, &t_now);
- interval = ((int64_t(t_now.tv_sec) * 1000000000LL + t_now.tv_nsec) -
- (int64_t(mt_pre.tv_sec) * 1000000000LL + mt_pre.tv_nsec));
+ t_now_ns = android::elapsedRealtimeNano();
+ interval = t_now_ns - mt_pre_ns;
if (interval < mStepCountPollTime) {
LOGV_IF(0,
@@ -4776,9 +4902,9 @@
interval, mStepCountPollTime);
return false;
} else {
- clock_gettime(CLOCK_BOOTTIME, &mt_pre);
- LOGV_IF(0, "Step Count previous time: %ld ms",
- mt_pre.tv_nsec / 1000);
+ mt_pre_ns = android::elapsedRealtimeNano();
+ LOGV_IF(0, "Step Count previous time: %lld ms",
+ mt_pre_ns / 1000000);
return true;
}
}
@@ -5223,7 +5349,6 @@
// get proper (in absolute) IIO path & build MPU's sysfs paths
inv_get_sysfs_path(sysfs_path);
- LOGV_IF(true, "njv Invensense sysfs path : %s", sysfs_path);
memcpy(mSysfsPath, sysfs_path, sizeof(sysfs_path));
sprintf(mpu.key, "%s%s", sysfs_path, "/key");
@@ -5845,7 +5970,7 @@
if (!timeout) {
mBatchEnabled &= ~(1 << what);
- mBatchDelays[what] = 1000000000L;
+ mBatchDelays[what] = 1000000000LL;
mDelays[what] = period_ns;
mBatchTimeouts[what] = 100000000000LL;
} else {
diff --git a/6515/libsensors_iio/MPLSensor.h b/6515/libsensors_iio/MPLSensor.h
index 13ba301..63df411 100644
--- a/6515/libsensors_iio/MPLSensor.h
+++ b/6515/libsensors_iio/MPLSensor.h
@@ -355,6 +355,7 @@
int mDmpStepCountEnabled;
uint32_t mEnabled;
+ uint32_t mEnabledCached;
uint32_t mBatchEnabled;
android::Vector<int> mFlushSensorEnabledVector;
uint32_t mOldBatchEnabledMask;
@@ -368,6 +369,7 @@
int64_t mBatchDelays[NumSensors];
int64_t mBatchTimeouts[NumSensors];
hfunc_t mHandlers[NumSensors];
+ int64_t mEnabledTime[NumSensors];
short mCachedGyroData[3];
long mCachedAccelData[3];
long mCachedCompassData[3];
@@ -537,6 +539,7 @@
long mInitial6QuatValue[4];
bool mFlushBatchSet;
uint32_t mSkipReadEvents;
+ uint32_t mSkipExecuteOnData;
bool mDataMarkerDetected;
bool mEmptyDataMarkerDetected;
int mDmpState;
diff --git a/6515/libsensors_iio/MPLSupport.cpp b/6515/libsensors_iio/MPLSupport.cpp
index 2954df3..dc40fab 100755
--- a/6515/libsensors_iio/MPLSupport.cpp
+++ b/6515/libsensors_iio/MPLSupport.cpp
@@ -150,7 +150,7 @@
sysfsfp = fopen(filename, "r");
if (sysfsfp != NULL) {
- if (fscanf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) {
+ if (fscanf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) {
res = errno;
LOGE("HAL:ERR open file %s to read with error %d", filename, res);
}
@@ -165,7 +165,7 @@
sysfsfp = fopen(filename, "r");
if (sysfsfp != NULL) {
- if (fscanf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {
+ if (fscanf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {
res = errno;
LOGE("HAL:ERR open file %s to read with error %d", filename, res);
}
@@ -178,7 +178,7 @@
int bytePosition = 0;
for (int index = 0; index < numElement; index++) {
for (int i = 0; i < 4; i++) {
- hex[bytePosition] = (int) ((quat[index] >> (4-1-i) * 8) & 0xFF);
+ hex[bytePosition] = (int) ((quat[index] >> (4-1-i) * 8) & 0xFF);
//LOGI("e%d quat[%d]: %x", index, bytePosition, hex[bytePosition]);
bytePosition++;
}
@@ -208,7 +208,7 @@
sysfsfp = fopen(filename, "w");
if (sysfsfp != NULL) {
- if (fprintf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {
+ if (fprintf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {
res = errno;
LOGE("HAL:ERR open file %s to write with error %d", filename, res);
}
@@ -216,7 +216,7 @@
return -res;
}
-int fill_dev_full_name_by_prefix(const char* dev_prefix,
+int fill_dev_full_name_by_prefix(const char* dev_prefix,
char *dev_full_name, int len)
{
char cand_name[20];
@@ -226,34 +226,34 @@
// try adding a number, 0-9
for(int cand_postfix = 0; cand_postfix < 10; cand_postfix++) {
snprintf(&cand_name[prefix_len],
- sizeof(cand_name) / sizeof(cand_name[0]),
+ sizeof(cand_name) / sizeof(cand_name[0]),
"%d", cand_postfix);
int dev_num = find_type_by_name(cand_name, "iio:device");
if (dev_num != -ENODEV) {
strncpy(dev_full_name, cand_name, len);
- return 0;
+ return 0;
}
}
// try adding a small letter, a-z
for(char cand_postfix = 'a'; cand_postfix <= 'z'; cand_postfix++) {
- snprintf(&cand_name[prefix_len],
- sizeof(cand_name) / sizeof(cand_name[0]),
+ snprintf(&cand_name[prefix_len],
+ sizeof(cand_name) / sizeof(cand_name[0]),
"%c", cand_postfix);
int dev_num = find_type_by_name(cand_name, "iio:device");
if (dev_num != -ENODEV) {
strncpy(dev_full_name, cand_name, len);
- return 0;
+ return 0;
}
}
// try adding a capital letter, A-Z
for(char cand_postfix = 'A'; cand_postfix <= 'Z'; cand_postfix++) {
snprintf(&cand_name[prefix_len],
- sizeof(cand_name) / sizeof(cand_name[0]),
+ sizeof(cand_name) / sizeof(cand_name[0]),
"%c", cand_postfix);
int dev_num = find_type_by_name(cand_name, "iio:device");
if (dev_num != -ENODEV) {
strncpy(dev_full_name, cand_name, len);
- return 0;
+ return 0;
}
}
return 1;
@@ -286,10 +286,10 @@
int fd;
char buf[sizeof(long) *4];
long data;
-
+
DIR *dp;
struct dirent *ep;
-
+
dp = opendir (sysfs_path);
if (dp != NULL)
@@ -316,7 +316,7 @@
LOGI("HAL DEBUG:sysfs:cat %s = %ld", full_path, data);
} else {
LOGV_IF(0,"HAL DEBUG: error reading %s", full_path);
- }
+ }
} else {
LOGV_IF(0,"HAL DEBUG: error opening %s", full_path);
}
diff --git a/6515/libsensors_iio/SensorBase.cpp b/6515/libsensors_iio/SensorBase.cpp
index 16057ff..f9825ad 100755
--- a/6515/libsensors_iio/SensorBase.cpp
+++ b/6515/libsensors_iio/SensorBase.cpp
@@ -25,6 +25,7 @@
#include <cutils/log.h>
#include <linux/input.h>
#include <string.h>
+#include <utils/SystemClock.h>
#include <cutils/properties.h>
@@ -142,10 +143,7 @@
int64_t SensorBase::getTimestamp()
{
- struct timespec t;
- t.tv_sec = t.tv_nsec = 0;
- clock_gettime(CLOCK_BOOTTIME, &t);
- return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec;
+ return android::elapsedRealtimeNano();
}
int SensorBase::openInput(const char *inputName)
diff --git a/6515/libsensors_iio/sensors_mpl.cpp b/6515/libsensors_iio/sensors_mpl.cpp
index c55e86c..b277e3b 100755
--- a/6515/libsensors_iio/sensors_mpl.cpp
+++ b/6515/libsensors_iio/sensors_mpl.cpp
@@ -32,16 +32,17 @@
#include <utils/Atomic.h>
#include <utils/Log.h>
+#include <utils/SystemClock.h>
#include "sensors.h"
#include "MPLSensor.h"
-/*
- * Vendor-defined Accel Load Calibration File Method
+/*
+ * Vendor-defined Accel Load Calibration File Method
* @param[out] Accel bias, length 3. In HW units scaled by 2^16 in body frame
* @return '0' for a successful load, '1' otherwise
* example: int AccelLoadConfig(long* offset);
- * End of Vendor-defined Accel Load Cal Method
+ * End of Vendor-defined Accel Load Cal Method
*/
/*****************************************************************************/
@@ -118,6 +119,7 @@
#if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP
int flush(int handle);
#endif
+ int64_t getTimestamp();
private:
enum {
@@ -136,7 +138,6 @@
/* Significant Motion wakelock support */
bool mSMDWakelockHeld;
-
};
/******************************************************************************/
@@ -153,7 +154,7 @@
/* For Vendor-defined Accel Calibration File Load
* Use the Following Constructor and Pass Your Load Cal File Function
- *
+ *
* MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig);
*/
@@ -183,7 +184,7 @@
mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd();
mPollFds[dmpPed].events = POLLPRI;
- mPollFds[dmpPed].revents = 0;
+ mPollFds[dmpPed].revents = 0;
}
sensors_poll_context_t::~sensors_poll_context_t() {
@@ -199,7 +200,7 @@
FUNC_LOG;
int err;
- err = mSensor->enable(handle, enabled);
+ err = mSensor->enable(handle, enabled);
return err;
}
@@ -209,6 +210,11 @@
return mSensor->setDelay(handle, ns);
}
+int64_t sensors_poll_context_t::getTimestamp()
+{
+ return android::elapsedRealtimeNano();
+}
+
int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count)
{
VHANDLER_LOG;
@@ -243,7 +249,7 @@
// look for new events
nb = poll(mPollFds, numSensorDrivers, polltime);
- LOGI_IF(0, "poll nb=%d, count=%d, pt=%d", nb, count, polltime);
+ LOGI_IF(0, "poll nb=%d, count=%d, pt=%d ts=%lld", nb, count, polltime, getTimestamp());
if (nb > 0) {
for (int i = 0; count && i < numSensorDrivers; i++) {
if (mPollFds[i].revents & (POLLIN | POLLPRI)) {
@@ -292,7 +298,7 @@
LOGI_IF(0, "sensors_mpl:readEvents() - "
"i=%d, nb=%d, count=%d, nbEvents=%d, "
"data->timestamp=%lld, data->data[0]=%f,",
- i, nb, count, nbEvents, data->timestamp,
+ i, nb, count, nbEvents, data->timestamp,
data->data[0]);
if (nb > 0) {
count -= nb;
@@ -342,7 +348,7 @@
return mSensor->query(what, value);
}
-int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns,
+int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns,
int64_t timeout)
{
FUNC_LOG;
