From 822ea53e105f41ec8b861f03ae9ea7f0111cef2b Mon Sep 17 00:00:00 2001 From: Nick Vaccaro Date: Mon, 26 Oct 2015 14:39:19 -0700 Subject: 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 --- 6515/libsensors_iio/Android.mk | 13 ++ 6515/libsensors_iio/MPLSensor.cpp | 287 ++++++++++++++++++++++++++---------- 6515/libsensors_iio/MPLSensor.h | 3 + 6515/libsensors_iio/MPLSupport.cpp | 30 ++-- 6515/libsensors_iio/SensorBase.cpp | 6 +- 6515/libsensors_iio/sensors_mpl.cpp | 26 ++-- 6 files changed, 255 insertions(+), 110 deletions(-) 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 @@ MAJOR_VERSION :=$(shell echo $(PLATFORM_VERSION) | cut -f1 -d.) 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 #include #include +#include #include "MPLSensor.h" #include "PressureSensor.IIO.secondary.h" @@ -75,7 +76,7 @@ static FILE *logfile = NULL; * 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 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * mDmpPedometerEnabled(0), mDmpStepCountEnabled(0), mEnabled(0), + mEnabledCached(0), mBatchEnabled(0), mOldBatchEnabledMask(0), mAccelInputReader(4), @@ -142,6 +144,7 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * mLeftOverBufferSize(0), mInitial6QuatValueAvailable(0), mSkipReadEvents(0), + mSkipExecuteOnData(0), mDataMarkerDetected(0), mEmptyDataMarkerDetected(0) { VFUNC_LOG; @@ -165,6 +168,7 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * 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 MPLSensor::setDmpFeature(int en) 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 @@ int MPLSensor::enableDmpPedometer(int en, int interruptMode) 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 @@ int MPLSensor::gyroHandler(sensors_event_t* s) 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 @@ int MPLSensor::rawGyroHandler(sensors_event_t* s) 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 @@ int MPLSensor::accelHandler(sensors_event_t* s) 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 @@ int MPLSensor::compassHandler(sensors_event_t* s) { 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 @@ int MPLSensor::compassHandler(sensors_event_t* s) 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 @@ int MPLSensor::rawCompassHandler(sensors_event_t* s) &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 @@ int MPLSensor::rawCompassHandler(sensors_event_t* s) 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 @@ int MPLSensor::rvHandler(sensors_event_t* s) &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 @@ int MPLSensor::grvHandler(sensors_event_t* s) #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 @@ int MPLSensor::laHandler(sensors_event_t* s) 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 @@ int MPLSensor::gravHandler(sensors_event_t* s) &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 @@ int MPLSensor::orienHandler(sensors_event_t* s) #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 @@ int MPLSensor::smHandler(sensors_event_t* s) 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 @@ int MPLSensor::gmHandler(sensors_event_t* s) 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 @@ int MPLSensor::gmHandler(sensors_event_t* s) &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 @@ int MPLSensor::psHandler(sensors_event_t* s) 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 @@ int MPLSensor::sdHandler(sensors_event_t* s) 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 @@ int MPLSensor::scHandler(sensors_event_t* s) #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 @@ int MPLSensor::enable(int32_t handle, int en) 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 @@ int MPLSensor::enable(int32_t handle, int en) (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 @@ int MPLSensor::enable(int32_t handle, int en) 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 @@ int MPLSensor::enable(int32_t handle, int en) (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 @@ int MPLSensor::enable(int32_t handle, int en) 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 @@ int MPLSensor::enable(int32_t handle, int en) } 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 @@ int MPLSensor::readEvents(sensors_event_t* data, int count) { VHANDLER_LOG; - inv_execute_on_data(); + if (!mSkipExecuteOnData) + inv_execute_on_data(); int numEventReceived = 0; @@ -3783,7 +3900,7 @@ int MPLSensor::readEvents(sensors_event_t* data, int count) } // 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 @@ int MPLSensor::readEvents(sensors_event_t* data, int count) // 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 @@ void MPLSensor::buildMpuEvent(void) 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 @@ LOGV_IF(INPUT_DATA, mFlushBatchSet = 1; } mEmptyDataMarkerDetected = 1; + mDataMarkerDetected = 1; } } @@ -3985,6 +4111,7 @@ LOGV_IF(INPUT_DATA, 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 @@ LOGV_IF(INPUT_DATA, 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 @@ LOGV_IF(INPUT_DATA, 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 @@ LOGV_IF(INPUT_DATA, "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 @@ LOGV_IF(INPUT_DATA, "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 @@ LOGV_IF(INPUT_DATA, "HAL:input inv_build_compass_of: %+8ld %+8ld %+8ld - %lld", mCachedCompassData[0], mCachedCompassData[1], mCachedCompassData[2], mCompassTimestamp); + mSkipExecuteOnData = 0; resetCompass(); } @@ -4317,6 +4445,7 @@ LOGV_IF(INPUT_DATA, "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld", mCachedCompassData[0], mCachedCompassData[1], mCachedCompassData[2], mCompassTimestamp); + mSkipExecuteOnData = 0; latestTimestamp = mCompassTimestamp; } @@ -4337,6 +4466,7 @@ LOGV_IF(INPUT_DATA, mCachedQuaternionData[0], mCachedQuaternionData[1], mCachedQuaternionData[2], mQuatSensorTimestamp); + mSkipExecuteOnData = 0; latestTimestamp = mQuatSensorTimestamp; } @@ -4356,6 +4486,7 @@ LOGV_IF(INPUT_DATA, "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 @@ LOGV_IF(INPUT_DATA, 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 @@ void MPLSensor::buildCompassEvent(void) 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 @@ int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) 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 @@ bool MPLSensor::hasStepCountPendingEvents(void) { 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 @@ bool MPLSensor::hasStepCountPendingEvents(void) 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 @@ int MPLSensor::inv_init_sysfs_attributes(void) // 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 @@ int MPLSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout) 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 @@ protected: int mDmpStepCountEnabled; uint32_t mEnabled; + uint32_t mEnabledCached; uint32_t mBatchEnabled; android::Vector mFlushSensorEnabledVector; uint32_t mOldBatchEnabledMask; @@ -368,6 +369,7 @@ protected: 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 @@ protected: 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 @@ int read_sysfs_int(char *filename, int *var) 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 @@ int read_sysfs_int64(char *filename, int64_t *var) 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 @@ void convert_long_to_hex_char(long* quat, unsigned char* hex, int numElement) 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 @@ int write_sysfs_longlong(char *filename, int64_t var) 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 @@ int write_sysfs_longlong(char *filename, int64_t var) 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 @@ int fill_dev_full_name_by_prefix(const char* dev_prefix, // 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 read_sysfs_dir(bool fileMode, char *sysfs_path) 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 @@ int read_sysfs_dir(bool fileMode, char *sysfs_path) 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 #include #include +#include #include @@ -142,10 +143,7 @@ bool SensorBase::hasPendingEvents() const 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 #include +#include #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 @@ struct sensors_poll_context_t { #if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP int flush(int handle); #endif + int64_t getTimestamp(); private: enum { @@ -136,7 +138,6 @@ private: /* Significant Motion wakelock support */ bool mSMDWakelockHeld; - }; /******************************************************************************/ @@ -153,7 +154,7 @@ sensors_poll_context_t::sensors_poll_context_t() { /* 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 @@ sensors_poll_context_t::sensors_poll_context_t() { 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 @@ int sensors_poll_context_t::activate(int handle, int enabled) { FUNC_LOG; int err; - err = mSensor->enable(handle, enabled); + err = mSensor->enable(handle, enabled); return err; } @@ -209,6 +210,11 @@ int sensors_poll_context_t::setDelay(int handle, int64_t ns) 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 @@ int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count) // 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 @@ int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count) 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 @@ int sensors_poll_context_t::query(int what, int* value) 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; -- cgit v1.2.3