diff options
author | Steve Kondik <steve@cyngn.com> | 2016-03-10 18:20:08 -0800 |
---|---|---|
committer | Steve Kondik <steve@cyngn.com> | 2016-03-10 18:20:08 -0800 |
commit | cb871446589029c6ab94829c610f668cb3aee6d9 (patch) | |
tree | b8532e4f5e7e8d59042aed7094fa06d940273c72 | |
parent | dfb04fdc823b8406a080764ae01235a3ee1ad06a (diff) | |
parent | f332a4e508430e15266bb7019f7132a7f7eb6dfb (diff) | |
download | android_hardware_invensense-cb871446589029c6ab94829c610f668cb3aee6d9.tar.gz android_hardware_invensense-cb871446589029c6ab94829c610f668cb3aee6d9.tar.bz2 android_hardware_invensense-cb871446589029c6ab94829c610f668cb3aee6d9.zip |
Merge tag 'android-6.0.1_r22' of https://android.googlesource.com/platform/hardware/invensense into cm-13.0staging/cm-13.0+r22
Android 6.0.1 release 22
-rwxr-xr-x | 6515/libsensors_iio/Android.mk | 13 | ||||
-rwxr-xr-x | 6515/libsensors_iio/InputEventReader.cpp | 1 | ||||
-rw-r--r-- | 6515/libsensors_iio/MPLSensor.cpp | 450 | ||||
-rw-r--r-- | 6515/libsensors_iio/MPLSensor.h | 6 | ||||
-rwxr-xr-x | 6515/libsensors_iio/MPLSupport.cpp | 30 | ||||
-rwxr-xr-x | 6515/libsensors_iio/SensorBase.cpp | 7 | ||||
-rwxr-xr-x | 6515/libsensors_iio/sensors_mpl.cpp | 35 |
7 files changed, 376 insertions, 166 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/InputEventReader.cpp b/6515/libsensors_iio/InputEventReader.cpp index 968e32e..5ec3e6c 100755 --- a/6515/libsensors_iio/InputEventReader.cpp +++ b/6515/libsensors_iio/InputEventReader.cpp @@ -20,6 +20,7 @@ #include <errno.h> #include <unistd.h> #include <poll.h> +#include <string.h> #include <sys/cdefs.h> #include <sys/types.h> diff --git a/6515/libsensors_iio/MPLSensor.cpp b/6515/libsensors_iio/MPLSensor.cpp index ad6d4e9..5a57d0b 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" @@ -62,6 +63,15 @@ #define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*)) +// query path to determine if vibrator is currently vibrating +#define VIBRATOR_ENABLE_FILE "/sys/class/timed_output/vibrator/enable" + + +// Minimum time after vibrator triggers SMD before SMD can be declared valid +// This allows 100mS for events to propogate +#define MIN_TRIGGER_TIME_AFTER_VIBRATOR_NS 100000000 + + /******************************************************************************/ /* MPL Interface */ /******************************************************************************/ @@ -75,7 +85,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 +107,7 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * mDmpPedometerEnabled(0), mDmpStepCountEnabled(0), mEnabled(0), + mEnabledCached(0), mBatchEnabled(0), mOldBatchEnabledMask(0), mAccelInputReader(4), @@ -117,20 +128,6 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * mAccelBiasApplied(false), mPendingMask(0), mSensorMask(0), - mMplFeatureActiveMask(0), - mFeatureActiveMask(0), - mDmpOn(0), - mPedUpdate(0), - mPressureUpdate(0), - mQuatSensorTimestamp(0), - mStepSensorTimestamp(0), - mLastStepCount(-1), - mLeftOverBufferSize(0), - mInitial6QuatValueAvailable(0), - mFlushBatchSet(0), - mSkipReadEvents(0), - mDataMarkerDetected(0), - mEmptyDataMarkerDetected(0), mGyroBatchRate(0), mAccelBatchRate(0), mCompassBatchRate(0), @@ -144,7 +141,21 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * mResetRate(0), mDataInterrupt(0), mFirstBatchCall(1), - mEnableCalled(1) { + mEnableCalled(1), + mMplFeatureActiveMask(0), + mFeatureActiveMask(0), + mDmpOn(0), + mPedUpdate(0), + mPressureUpdate(0), + mQuatSensorTimestamp(0), + mStepSensorTimestamp(0), + mLastStepCount(-1), + mLeftOverBufferSize(0), + mInitial6QuatValueAvailable(0), + mSkipReadEvents(0), + mSkipExecuteOnData(0), + mDataMarkerDetected(0), + mEmptyDataMarkerDetected(0) { VFUNC_LOG; inv_error_t rv; @@ -162,10 +173,13 @@ MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long * pthread_mutex_init(&mMplMutex, NULL); pthread_mutex_init(&mHALMutex, NULL); + mFlushBatchSet = 0; memset(mGyroOrientation, 0, sizeof(mGyroOrientation)); memset(mAccelOrientation, 0, sizeof(mAccelOrientation)); memset(mInitial6QuatValue, 0, sizeof(mInitial6QuatValue)); mFlushSensorEnabledVector.setCapacity(NumSensors); + memset(mEnabledTime, 0, sizeof(mEnabledTime)); + memset(mLastTimestamp, 0, sizeof(mLastTimestamp)); /* setup sysfs paths */ inv_init_sysfs_attributes(); @@ -1095,7 +1109,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)) { @@ -1822,7 +1836,7 @@ int MPLSensor::enableDmpPedometer(int en, int interruptMode) mFeatureActiveMask |= INV_DMP_PEDOMETER_STEP; } - clock_gettime(CLOCK_MONOTONIC, &mt_pre); + mt_pre_ns = android::elapsedRealtimeNano(); } else { if (interruptMode) { mFeatureActiveMask &= ~INV_DMP_PEDOMETER; @@ -2613,6 +2627,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; @@ -2629,6 +2649,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", @@ -2653,6 +2679,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); @@ -2664,6 +2696,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)); @@ -2671,17 +2704,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()) { @@ -2700,6 +2740,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", @@ -2710,7 +2756,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; } /* @@ -2730,7 +2776,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); @@ -2756,6 +2810,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); @@ -2774,6 +2834,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; @@ -2791,6 +2858,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; @@ -2809,6 +2883,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); @@ -2826,9 +2907,13 @@ int MPLSensor::smHandler(sensors_event_t* s) s->data[2] = 0.f; /* Capture timestamp in HAL */ - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &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); @@ -2840,6 +2925,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)); @@ -2848,6 +2934,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; @@ -2868,6 +2961,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; @@ -2885,9 +2986,7 @@ int MPLSensor::sdHandler(sensors_event_t* s) s->data[2] = 0.f; /* get current timestamp */ - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &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); @@ -2911,14 +3010,7 @@ int MPLSensor::scHandler(sensors_event_t* s) #endif if (s->timestamp == 0 && update) { - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &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; @@ -2939,10 +3031,10 @@ int MPLSensor::metaHandler(sensors_event_t* s, int flags) switch(flags) { case META_DATA_FLUSH_COMPLETE: s->type = SENSOR_TYPE_META_DATA; + s->version = META_DATA_VERSION; s->meta_data.what = flags; s->meta_data.sensor = mFlushSensorEnabledVector[0]; mFlushSensorEnabledVector.removeAt(0); - //mFlushBatchSet = 0; LOGV_IF(HANDLER_DATA, "HAL:flush complete data: type=%d what=%d, " "sensor=%d - %lld - %d", @@ -2975,7 +3067,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: @@ -2987,8 +3079,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, @@ -3002,6 +3102,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"; @@ -3011,6 +3118,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"; @@ -3097,6 +3208,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); @@ -3187,6 +3303,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); @@ -3717,7 +3843,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; @@ -3771,9 +3898,16 @@ int MPLSensor::readEvents(sensors_event_t* data, int count) update = readDmpPedometerEvents(data, count, ID_P, 1); mPedUpdate = 0; if(update == 1 && count > 0) { - data->timestamp = mStepSensorTimestamp; - count--; - numEventReceived++; + if (mLastTimestamp[i] != mStepSensorTimestamp) { + count--; + numEventReceived++; + data->timestamp = mStepSensorTimestamp; + data++; + mLastTimestamp[i] = mStepSensorTimestamp; + } else { + ALOGE("Event from type=%d with duplicate timestamp %lld discarded", + mPendingEvents[i].type, mStepSensorTimestamp); + } continue; } } else { @@ -3784,14 +3918,21 @@ 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); if (update && (count > 0)) { - *data++ = mPendingEvents[i]; - count--; - numEventReceived++; + // Discard any events with duplicate timestamps + if (mLastTimestamp[i] != mPendingEvents[i].timestamp) { + mLastTimestamp[i] = mPendingEvents[i].timestamp; + *data++ = mPendingEvents[i]; + count--; + numEventReceived++; + } else { + ALOGE("Event from type=%d with duplicate timestamp %lld discarded", + mPendingEvents[i].type, mStepSensorTimestamp); + } } } } @@ -3800,32 +3941,33 @@ int MPLSensor::readEvents(sensors_event_t* data, int count) // handle partial packet read and end marker // skip readEvents from hal_outputs - int flush_vec_size = mFlushSensorEnabledVector.size(); - if (flush_vec_size && mDataMarkerDetected && mFlushBatchSet) { - // 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) { - *data++ = mPendingFlushEvents[k]; + if (mFlushBatchSet && count>0 && !mFlushSensorEnabledVector.isEmpty()) { + while (mFlushBatchSet && count>0 && !mFlushSensorEnabledVector.isEmpty()) { + int sendEvent = metaHandler(&mPendingFlushEvents[0], META_DATA_FLUSH_COMPLETE); + if (sendEvent) { + LOGV_IF(ENG_VERBOSE, "Queueing flush complete for handle=%d", + mPendingFlushEvents[0].meta_data.sensor); + *data++ = mPendingFlushEvents[0]; count--; numEventReceived++; + } else { + LOGV_IF(ENG_VERBOSE, "sendEvent false, NOT queueing flush complete for handle=%d", + mPendingFlushEvents[0].meta_data.sensor); } - } - - if (!mEmptyDataMarkerDetected) { - // turn off sensors in data_builder - resetMplStates(); + mFlushBatchSet--; } // Double check flush status if (mFlushSensorEnabledVector.isEmpty()) { - mEmptyDataMarkerDetected = 0; + mEmptyDataMarkerDetected = 0; mDataMarkerDetected = 0; - mFlushBatchSet = 0; - LOGV_IF(0, "Flush completed"); + mFlushBatchSet = 0; + LOGV_IF(ENG_VERBOSE, "Flush completed"); } else { - LOGV_IF(0, "Flush is still active"); + LOGV_IF(ENG_VERBOSE, "Flush is still active"); } + } else if (mFlushBatchSet && mFlushSensorEnabledVector.isEmpty()) { + mFlushBatchSet = 0; } return numEventReceived; @@ -3855,6 +3997,18 @@ 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; + mDataMarkerDetected = 0; + mEmptyDataMarkerDetected = 0; + return; + } + lp_quaternion_on = isLowPowerQuatEnabled() && checkLPQuaternion(); sixAxis_quaternion_on = check6AxisQuatEnabled(); ped_quaternion_on = checkPedQuatEnabled(); @@ -3943,7 +4097,7 @@ LOGV_IF(INPUT_DATA, readCounter -= BYTES_PER_SENSOR; rdata += BYTES_PER_SENSOR; if (!mFlushSensorEnabledVector.isEmpty()) { - mFlushBatchSet = 1; + mFlushBatchSet++; } mDataMarkerDetected = 1; } @@ -3952,9 +4106,10 @@ LOGV_IF(INPUT_DATA, readCounter -= BYTES_PER_SENSOR; rdata += BYTES_PER_SENSOR; if (!mFlushSensorEnabledVector.isEmpty()) { - mFlushBatchSet = 1; + mFlushBatchSet++; } mEmptyDataMarkerDetected = 1; + mDataMarkerDetected = 1; } } @@ -3986,6 +4141,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; @@ -4019,19 +4175,18 @@ LOGV_IF(INPUT_DATA, LOGV_IF(ENG_VERBOSE && INPUT_DATA, "MARKER DETECTED:0x%x", data_format); readCounter -= BYTES_PER_SENSOR; if (!mFlushSensorEnabledVector.isEmpty()) { - mFlushBatchSet = 1; + mFlushBatchSet++; } 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); readCounter -= BYTES_PER_SENSOR; if (!mFlushSensorEnabledVector.isEmpty()) { - mFlushBatchSet = 1; + mFlushBatchSet++; } 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); @@ -4197,7 +4352,7 @@ LOGV_IF(INPUT_DATA, rdata += BYTES_PER_SENSOR; readCounter -= BYTES_PER_SENSOR; if (!mFlushSensorEnabledVector.isEmpty()) { - mFlushBatchSet = 1; + mFlushBatchSet++; } mDataMarkerDetected = 1; if (readCounter == 0) { @@ -4240,6 +4395,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]; @@ -4267,30 +4423,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) { @@ -4303,6 +4459,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(); } @@ -4318,6 +4475,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; } @@ -4338,6 +4496,7 @@ LOGV_IF(INPUT_DATA, mCachedQuaternionData[0], mCachedQuaternionData[1], mCachedQuaternionData[2], mQuatSensorTimestamp); + mSkipExecuteOnData = 0; latestTimestamp = mQuatSensorTimestamp; } @@ -4357,6 +4516,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; } @@ -4376,27 +4536,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 */ @@ -4407,6 +4566,9 @@ LOGV_IF(INPUT_DATA, LOGV_IF(INPUT_DATA, "HAL:input build step: 1 - %lld", mStepSensorTimestamp); } else { + LOGV_IF(ENG_VERBOSE, "Step data OUT OF ORDER, " + "mPedUpdate = 0x%x last = %lld, ts = %lld", + mPedUpdate, mStepSensorTimestamp, latestTimestamp); mPedUpdate = 0; } } @@ -4465,15 +4627,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); @@ -4694,9 +4855,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_MONOTONIC, &ts); - temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; + temp.timestamp = android::elapsedRealtimeNano(); *data++ = temp; count--; @@ -4764,12 +4923,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_MONOTONIC, &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, @@ -4777,9 +4935,9 @@ bool MPLSensor::hasStepCountPendingEvents(void) interval, mStepCountPollTime); return false; } else { - clock_gettime(CLOCK_MONOTONIC, &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; } } @@ -5759,7 +5917,11 @@ int MPLSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout) LOGV_IF(PROCESS_VERBOSE, "HAL:batch after applying upper and lower limit: %llu ns, (%.2f Hz)", - period_ns, 1000000000.f / period_ns); + period_ns, 1000000000.f / period_ns); + + LOGV_IF(PROCESS_VERBOSE, + "HAL:batch after applying upper and lower limit: %llu ns, (%.2f Hz)", + period_ns, 1000000000.f / period_ns); switch (what) { case Gyro: @@ -5841,7 +6003,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 { @@ -6101,7 +6263,6 @@ int MPLSensor::flush(int handle) } mFlushSensorEnabledVector.push_back(handle); - mFlushBatchSet = 0; /*write sysfs */ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", @@ -6361,29 +6522,46 @@ int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count) int res = 0; char dummy[4]; - int significantMotion; + int vibrator = 0; FILE *fp; int sensors = mEnabled; int numEventReceived = 0; int update = 0; + static int64_t lastVibTrigger = 0; - /* Technically this step is not necessary for now */ - /* In the future, we may have meaningful values */ - fp = fopen(mpu.event_smd, "r"); - if (fp == NULL) { - LOGE("HAL:cannot open event_smd"); - return 0; - } else { - if (fscanf(fp, "%d\n", &significantMotion) < 0) { - LOGE("HAL:cannot read event_smd"); - } - if (fclose(fp) < 0) { - LOGE("HAL:cannot close event_smd"); + if (mDmpSignificantMotionEnabled && count > 0) { + + // If vibrator is going off, ignore this event + fp = fopen(VIBRATOR_ENABLE_FILE, "r"); + if (fp != NULL) { + if (fscanf(fp, "%d\n", &vibrator) < 0) { + LOGE("HAL:cannot read %s", VIBRATOR_ENABLE_FILE); + } + if (fclose(fp) < 0) { + LOGE("HAL:cannot close %s", VIBRATOR_ENABLE_FILE); + } + if (vibrator != 0) { + lastVibTrigger = android::elapsedRealtimeNano(); + LOGV_IF(ENG_VERBOSE, "SMD triggered by vibrator, ignoring SMD event"); + return 0; + } else if (lastVibTrigger) { + // vibrator recently triggered SMD, discard related events + int64_t now = android::elapsedRealtimeNano(); + if ((now - lastVibTrigger) < MIN_TRIGGER_TIME_AFTER_VIBRATOR_NS) { + LOGV_IF(ENG_VERBOSE, "HAL: SMD triggered too close to vibrator (delta %lldnS), ignoring", + (now-lastVibTrigger)); + return 0; + } else { + LOGV_IF(ENG_VERBOSE, "HAL: SMD triggered %lld after vibrator (last %lld now %lld)", + now-lastVibTrigger, lastVibTrigger, now); + lastVibTrigger = 0; + } + } + } else { + LOGE("HAL:cannot open %s", VIBRATOR_ENABLE_FILE); } - } - if(mDmpSignificantMotionEnabled && count > 0) { - /* By implementation, smd is disabled once an event is triggered */ + /* By implementation, smd is disabled once an event is triggered */ sensors_event_t temp; /* Handles return event */ @@ -6543,7 +6721,7 @@ int MPLSensor::calcBatchDataRates(int64_t *gyro_rate, int64_t *accel_rate, int64 #ifdef ENABLE_PRESSURE int64_t pressureRate; #endif - int64_t quatRate; + int64_t quatRate = 0; int mplGyroRate; int mplAccelRate; diff --git a/6515/libsensors_iio/MPLSensor.h b/6515/libsensors_iio/MPLSensor.h index 13ba301..04a35ee 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<int> mFlushSensorEnabledVector; uint32_t mOldBatchEnabledMask; @@ -368,6 +369,8 @@ protected: int64_t mBatchDelays[NumSensors]; int64_t mBatchTimeouts[NumSensors]; hfunc_t mHandlers[NumSensors]; + int64_t mEnabledTime[NumSensors]; + int64_t mLastTimestamp[NumSensors]; short mCachedGyroData[3]; long mCachedAccelData[3]; long mCachedCompassData[3]; @@ -535,8 +538,9 @@ protected: char mLeftOverBuffer[1024]; bool mInitial6QuatValueAvailable; long mInitial6QuatValue[4]; - bool mFlushBatchSet; + int 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 544ac15..f9825ad 100755 --- a/6515/libsensors_iio/SensorBase.cpp +++ b/6515/libsensors_iio/SensorBase.cpp @@ -24,6 +24,8 @@ #include <sys/select.h> #include <cutils/log.h> #include <linux/input.h> +#include <string.h> +#include <utils/SystemClock.h> #include <cutils/properties.h> @@ -141,10 +143,7 @@ bool SensorBase::hasPendingEvents() const int64_t SensorBase::getTimestamp() { - struct timespec t; - t.tv_sec = t.tv_nsec = 0; - clock_gettime(CLOCK_MONOTONIC, &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 7e7b06b..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 */ /*****************************************************************************/ @@ -81,6 +82,14 @@ static struct hw_module_methods_t sensors_module_methods = { open: open_sensors }; +static int sensors_set_operation_mode(unsigned int mode) +{ + LOGI("%s", __FUNCTION__); + LOGI("%s: stub function: ignoring mode request (%d)", __FUNCTION__, + mode); + return 0; +} + struct sensors_module_t HAL_MODULE_INFO_SYM = { common: { tag: HARDWARE_MODULE_TAG, @@ -94,6 +103,7 @@ struct sensors_module_t HAL_MODULE_INFO_SYM = { reserved: {0} }, get_sensors_list: sensors__get_sensors_list, + set_operation_mode: sensors_set_operation_mode }; struct sensors_poll_context_t { @@ -109,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 { @@ -127,7 +138,6 @@ private: /* Significant Motion wakelock support */ bool mSMDWakelockHeld; - }; /******************************************************************************/ @@ -144,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); */ @@ -174,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() { @@ -190,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; } @@ -200,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; @@ -234,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)) { @@ -283,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; @@ -333,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; |