| /* |
| * Copyright (C) 2014 Invensense, Inc. |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| */ |
| |
| #define LOG_NDEBUG 0 |
| |
| //see also the EXTRA_VERBOSE define in the MPLSensor.h header file |
| |
| #include <fcntl.h> |
| #include <errno.h> |
| #include <math.h> |
| #include <float.h> |
| #include <poll.h> |
| #include <unistd.h> |
| #include <dirent.h> |
| #include <stdlib.h> |
| #include <sys/select.h> |
| #include <sys/syscall.h> |
| #include <dlfcn.h> |
| #include <pthread.h> |
| #include <cutils/atomic.h> |
| #include <cutils/log.h> |
| #include <utils/KeyedVector.h> |
| #include <utils/Vector.h> |
| #include <utils/String8.h> |
| #include <string.h> |
| #include <linux/input.h> |
| #include <utils/SystemClock.h> |
| |
| #include "MPLSensor.h" |
| #include "PressureSensor.IIO.secondary.h" |
| #include "MPLSupport.h" |
| #include "sensor_params.h" |
| |
| #include "invensense.h" |
| #include "invensense_adv.h" |
| #include "ml_stored_data.h" |
| #include "ml_load_dmp.h" |
| #include "ml_sysfs_helper.h" |
| |
| #define ENABLE_MULTI_RATE |
| // #define TESTING |
| #define USE_LPQ_AT_FASTEST |
| |
| #ifdef THIRD_PARTY_ACCEL |
| #pragma message("HAL:build third party accel support") |
| #define USE_THIRD_PARTY_ACCEL (1) |
| #else |
| #define USE_THIRD_PARTY_ACCEL (0) |
| #endif |
| |
| #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 */ |
| /******************************************************************************/ |
| |
| //#define INV_PLAYBACK_DBG |
| #ifdef INV_PLAYBACK_DBG |
| static FILE *logfile = NULL; |
| #endif |
| |
| /******************************************************************************* |
| * MPLSensor class implementation |
| ******************************************************************************/ |
| |
| static int64_t mt_pre_ns; |
| |
| // following extended initializer list would only be available with -std=c++11 |
| // or -std=gnu+11 |
| MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *)) |
| : SensorBase(NULL, NULL), |
| mMasterSensorMask(INV_ALL_SENSORS), |
| mLocalSensorMask(0), |
| mPollTime(-1), |
| mStepCountPollTime(-1), |
| mHaveGoodMpuCal(0), |
| mGyroAccuracy(0), |
| mAccelAccuracy(0), |
| mCompassAccuracy(0), |
| dmp_orient_fd(-1), |
| mDmpOrientationEnabled(0), |
| dmp_sign_motion_fd(-1), |
| mDmpSignificantMotionEnabled(0), |
| dmp_pedometer_fd(-1), |
| mDmpPedometerEnabled(0), |
| mDmpStepCountEnabled(0), |
| mEnabled(0), |
| mEnabledCached(0), |
| mBatchEnabled(0), |
| mOldBatchEnabledMask(0), |
| mAccelInputReader(4), |
| mGyroInputReader(32), |
| mTempScale(0), |
| mTempOffset(0), |
| mTempCurrentTime(0), |
| mAccelScale(2), |
| mAccelSelfTestScale(2), |
| mGyroScale(2000), |
| mGyroSelfTestScale(2000), |
| mCompassScale(0), |
| mFactoryGyroBiasAvailable(false), |
| mGyroBiasAvailable(false), |
| mGyroBiasApplied(false), |
| mFactoryAccelBiasAvailable(false), |
| mAccelBiasAvailable(false), |
| mAccelBiasApplied(false), |
| mPendingMask(0), |
| mSensorMask(0), |
| mGyroBatchRate(0), |
| mAccelBatchRate(0), |
| mCompassBatchRate(0), |
| mPressureBatchRate(0), |
| mQuatBatchRate(0), |
| mGyroRate(0), |
| mAccelRate(0), |
| mCompassRate(0), |
| mPressureRate(0), |
| mQuatRate(0), |
| mResetRate(0), |
| mDataInterrupt(0), |
| mFirstBatchCall(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; |
| int i, fd; |
| char *port = NULL; |
| char *ver_str; |
| unsigned long mSensorMask; |
| int res; |
| FILE *fptr; |
| |
| mCompassSensor = compass; |
| |
| LOGV_IF(EXTRA_VERBOSE, |
| "HAL:MPLSensor constructor : NumSensors = %d", NumSensors); |
| |
| 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(); |
| |
| /* get chip name */ |
| if (inv_get_chip_name(chip_ID) != INV_SUCCESS) { |
| LOGE("HAL:ERR- Failed to get chip ID\n"); |
| } else { |
| LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID); |
| } |
| |
| enable_iio_sysfs(); |
| |
| #ifdef ENABLE_PRESSURE |
| /* instantiate pressure sensor on secondary bus */ |
| mPressureSensor = new PressureSensor((const char*)mSysfsPath); |
| #endif |
| |
| /* reset driver master enable */ |
| masterEnable(0); |
| |
| /* Load DMP image if capable, ie. MPU6515 */ |
| loadDMP(); |
| |
| /* open temperature fd for temp comp */ |
| LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature); |
| gyro_temperature_fd = open(mpu.temperature, O_RDONLY); |
| if (gyro_temperature_fd == -1) { |
| LOGE("HAL:could not open temperature node"); |
| } else { |
| LOGV_IF(EXTRA_VERBOSE, |
| "HAL:temperature_fd opened: %s", mpu.temperature); |
| } |
| |
| /* read gyro FSR to calculate accel scale later */ |
| char gyroBuf[5]; |
| int count = 0; |
| LOGV_IF(SYSFS_VERBOSE, |
| "HAL:sysfs:cat %s (%lld)", mpu.gyro_fsr, getTimestamp()); |
| |
| fd = open(mpu.gyro_fsr, O_RDONLY); |
| if(fd < 0) { |
| LOGE("HAL:Error opening gyro FSR"); |
| } else { |
| memset(gyroBuf, 0, sizeof(gyroBuf)); |
| count = read_attribute_sensor(fd, gyroBuf, sizeof(gyroBuf)); |
| if(count < 1) { |
| LOGE("HAL:Error reading gyro FSR"); |
| } else { |
| count = sscanf(gyroBuf, "%ld", &mGyroScale); |
| if(count) |
| LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro FSR used %ld", mGyroScale); |
| } |
| close(fd); |
| } |
| |
| /* read gyro self test scale used to calculate factory cal bias later */ |
| char gyroScale[5]; |
| LOGV_IF(SYSFS_VERBOSE, |
| "HAL:sysfs:cat %s (%lld)", mpu.in_gyro_self_test_scale, getTimestamp()); |
| fd = open(mpu.in_gyro_self_test_scale, O_RDONLY); |
| if(fd < 0) { |
| LOGE("HAL:Error opening gyro self test scale"); |
| } else { |
| memset(gyroScale, 0, sizeof(gyroScale)); |
| count = read_attribute_sensor(fd, gyroScale, sizeof(gyroScale)); |
| if(count < 1) { |
| LOGE("HAL:Error reading gyro self test scale"); |
| } else { |
| count = sscanf(gyroScale, "%ld", &mGyroSelfTestScale); |
| if(count) |
| LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro self test scale used %ld", mGyroSelfTestScale); |
| } |
| close(fd); |
| } |
| |
| /* open Factory Gyro Bias fd */ |
| /* mFactoryGyBias contains bias values that will be used for device offset */ |
| memset(mFactoryGyroBias, 0, sizeof(mFactoryGyroBias)); |
| LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro x offset path: %s", mpu.in_gyro_x_offset); |
| LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro y offset path: %s", mpu.in_gyro_y_offset); |
| LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro z offset path: %s", mpu.in_gyro_z_offset); |
| gyro_x_offset_fd = open(mpu.in_gyro_x_offset, O_RDWR); |
| gyro_y_offset_fd = open(mpu.in_gyro_y_offset, O_RDWR); |
| gyro_z_offset_fd = open(mpu.in_gyro_z_offset, O_RDWR); |
| if (gyro_x_offset_fd == -1 || |
| gyro_y_offset_fd == -1 || gyro_z_offset_fd == -1) { |
| LOGE("HAL:could not open factory gyro calibrated bias"); |
| } else { |
| LOGV_IF(EXTRA_VERBOSE, |
| "HAL:gyro_offset opened"); |
| } |
| |
| /* open Gyro Bias fd */ |
| /* mGyroBias contains bias values that will be used for framework */ |
| /* mGyroChipBias contains bias values that will be used for dmp */ |
| memset(mGyroBias, 0, sizeof(mGyroBias)); |
| memset(mGyroChipBias, 0, sizeof(mGyroChipBias)); |
| LOGV_IF(EXTRA_VERBOSE, "HAL: gyro x dmp bias path: %s", mpu.in_gyro_x_dmp_bias); |
| LOGV_IF(EXTRA_VERBOSE, "HAL: gyro y dmp bias path: %s", mpu.in_gyro_y_dmp_bias); |
| LOGV_IF(EXTRA_VERBOSE, "HAL: gyro z dmp bias path: %s", mpu.in_gyro_z_dmp_bias); |
| gyro_x_dmp_bias_fd = open(mpu.in_gyro_x_dmp_bias, O_RDWR); |
| gyro_y_dmp_bias_fd = open(mpu.in_gyro_y_dmp_bias, O_RDWR); |
| gyro_z_dmp_bias_fd = open(mpu.in_gyro_z_dmp_bias, O_RDWR); |
| if (gyro_x_dmp_bias_fd == -1 || |
| gyro_y_dmp_bias_fd == -1 || gyro_z_dmp_bias_fd == -1) { |
| LOGE("HAL:could not open gyro DMP calibrated bias"); |
| } else { |
| LOGV_IF(EXTRA_VERBOSE, |
| "HAL:gyro_dmp_bias opened"); |
| } |
| |
| /* read accel FSR to calcuate accel scale later */ |
| if (USE_THIRD_PARTY_ACCEL == 0) { |
| char buf[3]; |
| int count = 0; |
| LOGV_IF(SYSFS_VERBOSE, |
| "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp()); |
| |
| fd = open(mpu.accel_fsr, O_RDONLY); |
| if(fd < 0) { |
| LOGE("HAL:Error opening accel FSR"); |
| } else { |
| memset(buf, 0, sizeof(buf)); |
| count = read_attribute_sensor(fd, buf, sizeof(buf)); |
| if(count < 1) { |
| LOGE("HAL:Error reading accel FSR"); |
| } else { |
| count = sscanf(buf, "%d", &mAccelScale); |
| if(count) |
| LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale); |
| } |
| close(fd); |
| } |
| |
| /* read accel self test scale used to calculate factory cal bias later */ |
| char accelScale[5]; |
| LOGV_IF(SYSFS_VERBOSE, |
| "HAL:sysfs:cat %s (%lld)", mpu.in_accel_self_test_scale, getTimestamp()); |
| fd = open(mpu.in_accel_self_test_scale, O_RDONLY); |
| if(fd < 0) { |
| LOGE("HAL:Error opening gyro self test scale"); |
| } else { |
| memset(accelScale, 0, sizeof(accelScale)); |
| count = read_attribute_sensor(fd, accelScale, sizeof(accelScale)); |
| if(count < 1) { |
| LOGE("HAL:Error reading accel self test scale"); |
| } else { |
| count = sscanf(accelScale, "%ld", &mAccelSelfTestScale); |
| if(count) |
| LOGV_IF(EXTRA_VERBOSE, "HAL:Accel self test scale used %ld", mAccelSelfTestScale); |
| } |
| close(fd); |
| } |
| |
| /* open Factory Accel Bias fd */ |
| /* mFactoryAccelBias contains bias values that will be used for device offset */ |
| memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias)); |
| LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel x offset path: %s", mpu.in_accel_x_offset); |
| LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel y offset path: %s", mpu.in_accel_y_offset); |
| LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel z offset path: %s", mpu.in_accel_z_offset); |
| accel_x_offset_fd = open(mpu.in_accel_x_offset, O_RDWR); |
| accel_y_offset_fd = open(mpu.in_accel_y_offset, O_RDWR); |
| accel_z_offset_fd = open(mpu.in_accel_z_offset, O_RDWR); |
| if (accel_x_offset_fd == -1 || |
| accel_y_offset_fd == -1 || accel_z_offset_fd == -1) { |
| LOGE("HAL:could not open factory accel calibrated bias"); |
| } else { |
| LOGV_IF(EXTRA_VERBOSE, |
| "HAL:accel_offset opened"); |
| } |
| |
| /* open Accel Bias fd */ |
| /* mAccelBias contains bias that will be used for dmp */ |
| memset(mAccelBias, 0, sizeof(mAccelBias)); |
| LOGV_IF(EXTRA_VERBOSE, "HAL:accel x dmp bias path: %s", mpu.in_accel_x_dmp_bias); |
| LOGV_IF(EXTRA_VERBOSE, "HAL:accel y dmp bias path: %s", mpu.in_accel_y_dmp_bias); |
| LOGV_IF(EXTRA_VERBOSE, "HAL:accel z dmp bias path: %s", mpu.in_accel_z_dmp_bias); |
| accel_x_dmp_bias_fd = open(mpu.in_accel_x_dmp_bias, O_RDWR); |
| accel_y_dmp_bias_fd = open(mpu.in_accel_y_dmp_bias, O_RDWR); |
| accel_z_dmp_bias_fd = open(mpu.in_accel_z_dmp_bias, O_RDWR); |
| if (accel_x_dmp_bias_fd == -1 || |
| accel_y_dmp_bias_fd == -1 || accel_z_dmp_bias_fd == -1) { |
| LOGE("HAL:could not open accel DMP calibrated bias"); |
| } else { |
| LOGV_IF(EXTRA_VERBOSE, |
| "HAL:accel_dmp_bias opened"); |
| } |
| } |
| |
| dmp_sign_motion_fd = open(mpu.event_smd, O_RDONLY | O_NONBLOCK); |
| if (dmp_sign_motion_fd < 0) { |
| LOGE("HAL:ERR couldn't open dmp_sign_motion node"); |
| } else { |
| LOGV_IF(ENG_VERBOSE, |
| "HAL:dmp_sign_motion_fd opened : %d", dmp_sign_motion_fd); |
| } |
| |
| /* the following threshold can be modified for SMD sensitivity */ |
| int motionThreshold = 3000; |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| motionThreshold, mpu.smd_threshold, getTimestamp()); |
| res = write_sysfs_int(mpu.smd_threshold, motionThreshold); |
| |
| #if 0 |
| int StepCounterThreshold = 5; |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| StepCounterThreshold, mpu.pedometer_step_thresh, getTimestamp()); |
| res = write_sysfs_int(mpu.pedometer_step_thresh, StepCounterThreshold); |
| #endif |
| |
| dmp_pedometer_fd = open(mpu.event_pedometer, O_RDONLY | O_NONBLOCK); |
| if (dmp_pedometer_fd < 0) { |
| LOGE("HAL:ERR couldn't open dmp_pedometer node"); |
| } else { |
| LOGV_IF(ENG_VERBOSE, |
| "HAL:dmp_pedometer_fd opened : %d", dmp_pedometer_fd); |
| } |
| |
| initBias(); |
| |
| (void)inv_get_version(&ver_str); |
| LOGI("%s\n", ver_str); |
| |
| /* setup MPL */ |
| inv_constructor_init(); |
| |
| #ifdef INV_PLAYBACK_DBG |
| LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging"); |
| logfile = fopen("/data/playback.bin", "w+"); |
| if (logfile) |
| inv_turn_on_data_logging(logfile); |
| #endif |
| |
| /* setup orientation matrix and scale */ |
| inv_set_device_properties(); |
| |
| /* initialize sensor data */ |
| memset(mPendingEvents, 0, sizeof(mPendingEvents)); |
| memset(mPendingFlushEvents, 0, sizeof(mPendingFlushEvents)); |
| |
| mPendingEvents[RotationVector].version = sizeof(sensors_event_t); |
| mPendingEvents[RotationVector].sensor = ID_RV; |
| mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; |
| mPendingEvents[RotationVector].acceleration.status |
| = SENSOR_STATUS_ACCURACY_HIGH; |
| |
| mPendingEvents[GameRotationVector].version = sizeof(sensors_event_t); |
| mPendingEvents[GameRotationVector].sensor = ID_GRV; |
| mPendingEvents[GameRotationVector].type = SENSOR_TYPE_GAME_ROTATION_VECTOR; |
| mPendingEvents[GameRotationVector].acceleration.status |
| = SENSOR_STATUS_ACCURACY_HIGH; |
| |
| mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); |
| mPendingEvents[LinearAccel].sensor = ID_LA; |
| mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; |
| mPendingEvents[LinearAccel].acceleration.status |
| = SENSOR_STATUS_ACCURACY_HIGH; |
| |
| mPendingEvents[Gravity].version = sizeof(sensors_event_t); |
| mPendingEvents[Gravity].sensor = ID_GR; |
| mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; |
| mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; |
| |
| mPendingEvents[Gyro].version = sizeof(sensors_event_t); |
| mPendingEvents[Gyro].sensor = ID_GY; |
| mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; |
| mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; |
| |
| mPendingEvents[RawGyro].version = sizeof(sensors_event_t); |
| mPendingEvents[RawGyro].sensor = ID_RG; |
| mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED; |
| mPendingEvents[RawGyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; |
| |
| mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); |
| mPendingEvents[Accelerometer].sensor = ID_A; |
| mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; |
| mPendingEvents[Accelerometer].acceleration.status |
| = SENSOR_STATUS_ACCURACY_HIGH; |
| |
| /* Invensense compass calibration */ |
| mPendingEvents[MagneticField].version = sizeof(sensors_event_t); |
| mPendingEvents[MagneticField].sensor = ID_M; |
| mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; |
| mPendingEvents[MagneticField].magnetic.status = |
| SENSOR_STATUS_ACCURACY_HIGH; |
| |
| mPendingEvents[RawMagneticField].version = sizeof(sensors_event_t); |
| mPendingEvents[RawMagneticField].sensor = ID_RM; |
| mPendingEvents[RawMagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED; |
| mPendingEvents[RawMagneticField].magnetic.status = |
| SENSOR_STATUS_ACCURACY_HIGH; |
| |
| #ifdef ENABLE_PRESSURE |
| mPendingEvents[Pressure].version = sizeof(sensors_event_t); |
| mPendingEvents[Pressure].sensor = ID_PS; |
| mPendingEvents[Pressure].type = SENSOR_TYPE_PRESSURE; |
| mPendingEvents[Pressure].magnetic.status = |
| SENSOR_STATUS_ACCURACY_HIGH; |
| #endif |
| |
| mPendingEvents[Orientation].version = sizeof(sensors_event_t); |
| mPendingEvents[Orientation].sensor = ID_O; |
| mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; |
| mPendingEvents[Orientation].orientation.status |
| = SENSOR_STATUS_ACCURACY_HIGH; |
| |
| mPendingEvents[GeomagneticRotationVector].version = sizeof(sensors_event_t); |
| mPendingEvents[GeomagneticRotationVector].sensor = ID_GMRV; |
| mPendingEvents[GeomagneticRotationVector].type |
| = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR; |
| mPendingEvents[GeomagneticRotationVector].acceleration.status |
| = SENSOR_STATUS_ACCURACY_HIGH; |
| |
| mSmEvents.version = sizeof(sensors_event_t); |
| mSmEvents.sensor = ID_SM; |
| mSmEvents.type = SENSOR_TYPE_SIGNIFICANT_MOTION; |
| mSmEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE; |
| |
| mSdEvents.version = sizeof(sensors_event_t); |
| mSdEvents.sensor = ID_P; |
| mSdEvents.type = SENSOR_TYPE_STEP_DETECTOR; |
| mSdEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE; |
| |
| mScEvents.version = sizeof(sensors_event_t); |
| mScEvents.sensor = ID_SC; |
| mScEvents.type = SENSOR_TYPE_STEP_COUNTER; |
| mScEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE; |
| |
| /* Event Handlers for HW and Virtual Sensors */ |
| mHandlers[RotationVector] = &MPLSensor::rvHandler; |
| mHandlers[GameRotationVector] = &MPLSensor::grvHandler; |
| mHandlers[LinearAccel] = &MPLSensor::laHandler; |
| mHandlers[Gravity] = &MPLSensor::gravHandler; |
| mHandlers[Gyro] = &MPLSensor::gyroHandler; |
| mHandlers[RawGyro] = &MPLSensor::rawGyroHandler; |
| mHandlers[Accelerometer] = &MPLSensor::accelHandler; |
| mHandlers[MagneticField] = &MPLSensor::compassHandler; |
| mHandlers[RawMagneticField] = &MPLSensor::rawCompassHandler; |
| mHandlers[Orientation] = &MPLSensor::orienHandler; |
| mHandlers[GeomagneticRotationVector] = &MPLSensor::gmHandler; |
| #ifdef ENABLE_PRESSURE |
| mHandlers[Pressure] = &MPLSensor::psHandler; |
| #endif |
| |
| /* initialize delays to reasonable values */ |
| for (int i = 0; i < NumSensors; i++) { |
| mDelays[i] = 1000000000LL; |
| mBatchDelays[i] = 1000000000LL; |
| mBatchTimeouts[i] = 100000000000LL; |
| } |
| |
| /* initialize Compass Bias */ |
| memset(mCompassBias, 0, sizeof(mCompassBias)); |
| |
| /* initialize Factory Accel Bias */ |
| memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias)); |
| |
| /* initialize Gyro Bias */ |
| memset(mGyroBias, 0, sizeof(mGyroBias)); |
| memset(mGyroChipBias, 0, sizeof(mGyroChipBias)); |
| |
| /* load calibration file from /data/inv_cal_data.bin */ |
| rv = inv_load_calibration(); |
| if(rv == INV_SUCCESS) { |
| LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded"); |
| /* Get initial values */ |
| getCompassBias(); |
| getGyroBias(); |
| if (mGyroBiasAvailable) { |
| setGyroBias(); |
| } |
| getAccelBias(); |
| getFactoryGyroBias(); |
| if (mFactoryGyroBiasAvailable) { |
| setFactoryGyroBias(); |
| } |
| getFactoryAccelBias(); |
| if (mFactoryAccelBiasAvailable) { |
| setFactoryAccelBias(); |
| } |
| } |
| else |
| LOGE("HAL:Could not open or load MPL calibration file (%d)", rv); |
| |
| /* takes external accel calibration load workflow */ |
| if( m_pt2AccelCalLoadFunc != NULL) { |
| long accel_offset[3]; |
| long tmp_offset[3]; |
| int result = m_pt2AccelCalLoadFunc(accel_offset); |
| if(result) |
| LOGW("HAL:Vendor accelerometer calibration file load failed %d\n", |
| result); |
| else { |
| LOGW("HAL:Vendor accelerometer calibration file successfully " |
| "loaded"); |
| inv_get_mpl_accel_bias(tmp_offset, NULL); |
| LOGV_IF(PROCESS_VERBOSE, |
| "HAL:Original accel offset, %ld, %ld, %ld\n", |
| tmp_offset[0], tmp_offset[1], tmp_offset[2]); |
| inv_set_accel_bias_mask(accel_offset, mAccelAccuracy,4); |
| inv_get_mpl_accel_bias(tmp_offset, NULL); |
| LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n", |
| tmp_offset[0], tmp_offset[1], tmp_offset[2]); |
| } |
| } |
| /* end of external accel calibration load workflow */ |
| |
| /* disable all sensors and features */ |
| masterEnable(0); |
| enableGyro(0); |
| enableLowPowerAccel(0); |
| enableAccel(0); |
| enableCompass(0,0); |
| #ifdef ENABLE_PRESSURE |
| enablePressure(0); |
| #endif |
| enableBatch(0); |
| |
| if (isLowPowerQuatEnabled()) { |
| enableLPQuaternion(0); |
| } |
| |
| if (isDmpDisplayOrientationOn()) { |
| // open DMP Orient Fd |
| openDmpOrientFd(); |
| enableDmpOrientation(!isDmpScreenAutoRotationEnabled()); |
| } |
| } |
| |
| void MPLSensor::enable_iio_sysfs(void) |
| { |
| VFUNC_LOG; |
| |
| char iio_device_node[MAX_CHIP_ID_LEN]; |
| FILE *tempFp = NULL; |
| |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)", |
| mpu.in_timestamp_en, getTimestamp()); |
| // Either fopen()/open() are okay for sysfs access |
| // developers could choose what they want |
| // with fopen(), the benefit is that fprintf()/fscanf() are available |
| tempFp = fopen(mpu.in_timestamp_en, "w"); |
| if (tempFp == NULL) { |
| LOGE("HAL:could not open timestamp enable"); |
| } else { |
| if(fprintf(tempFp, "%d", 1) < 0) { |
| LOGE("HAL:could not enable timestamp"); |
| } |
| if(fclose(tempFp) < 0) { |
| LOGE("HAL:could not close timestamp"); |
| } |
| } |
| |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp()); |
| tempFp = fopen(mpu.buffer_length, "w"); |
| if (tempFp == NULL) { |
| LOGE("HAL:could not open buffer length"); |
| } else { |
| if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) { |
| LOGE("HAL:could not write buffer length"); |
| } |
| if (fclose(tempFp) < 0) { |
| LOGE("HAL:could not close buffer length"); |
| } |
| } |
| |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 1, mpu.chip_enable, getTimestamp()); |
| tempFp = fopen(mpu.chip_enable, "w"); |
| if (tempFp == NULL) { |
| LOGE("HAL:could not open chip enable"); |
| } else { |
| if (fprintf(tempFp, "%d", 1) < 0) { |
| LOGE("HAL:could not write chip enable"); |
| } |
| if (fclose(tempFp) < 0) { |
| LOGE("HAL:could not close chip enable"); |
| } |
| } |
| |
| inv_get_iio_device_node(iio_device_node); |
| iio_fd = open(iio_device_node, O_RDONLY); |
| if (iio_fd < 0) { |
| LOGE("HAL:could not open iio device node"); |
| } else { |
| LOGV_IF(ENG_VERBOSE, "HAL:iio iio_fd opened : %d", iio_fd); |
| } |
| } |
| |
| int MPLSensor::inv_constructor_init(void) |
| { |
| VFUNC_LOG; |
| |
| inv_error_t result = inv_init_mpl(); |
| if (result) { |
| LOGE("HAL:inv_init_mpl() failed"); |
| return result; |
| } |
| result = inv_constructor_default_enable(); |
| result = inv_start_mpl(); |
| if (result) { |
| LOGE("HAL:inv_start_mpl() failed"); |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| |
| return result; |
| } |
| |
| int MPLSensor::inv_constructor_default_enable(void) |
| { |
| VFUNC_LOG; |
| |
| inv_error_t result; |
| |
| /******************************************************************************* |
| |
| ******************************************************************************** |
| |
| The InvenSense binary file (libmplmpu.so) is subject to Google's standard terms |
| and conditions as accepted in the click-through agreement required to download |
| this library. |
| The library includes, but is not limited to the following function calls: |
| inv_enable_quaternion(). |
| |
| ANY VIOLATION OF SUCH TERMS AND CONDITIONS WILL BE STRICTLY ENFORCED. |
| |
| ******************************************************************************** |
| |
| *******************************************************************************/ |
| |
| result = inv_enable_quaternion(); |
| if (result) { |
| LOGE("HAL:Cannot enable quaternion\n"); |
| return result; |
| } |
| |
| result = inv_enable_in_use_auto_calibration(); |
| if (result) { |
| return result; |
| } |
| |
| result = inv_enable_fast_nomot(); |
| if (result) { |
| return result; |
| } |
| |
| result = inv_enable_gyro_tc(); |
| if (result) { |
| return result; |
| } |
| |
| result = inv_enable_hal_outputs(); |
| if (result) { |
| return result; |
| } |
| |
| if (!mCompassSensor->providesCalibration()) { |
| /* Invensense compass calibration */ |
| LOGV_IF(ENG_VERBOSE, "HAL:Invensense vector compass cal enabled"); |
| result = inv_enable_vector_compass_cal(); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } else { |
| mMplFeatureActiveMask |= INV_COMPASS_CAL; |
| } |
| // specify MPL's trust weight, used by compass algorithms |
| inv_vector_compass_cal_sensitivity(3); |
| |
| /* disabled by default |
| result = inv_enable_compass_bias_w_gyro(); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| */ |
| |
| result = inv_enable_heading_from_gyro(); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| |
| result = inv_enable_magnetic_disturbance(); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| //inv_enable_magnetic_disturbance_logging(); |
| } |
| |
| result = inv_enable_9x_sensor_fusion(); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } else { |
| // 9x sensor fusion enables Compass fit |
| mMplFeatureActiveMask |= INV_COMPASS_FIT; |
| } |
| |
| result = inv_enable_no_gyro_fusion(); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| |
| return result; |
| } |
| |
| /* TODO: create function pointers to calculate scale */ |
| void MPLSensor::inv_set_device_properties(void) |
| { |
| VFUNC_LOG; |
| |
| unsigned short orient; |
| |
| inv_get_sensors_orientation(); |
| |
| inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE); |
| inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE); |
| |
| /* gyro setup */ |
| orient = inv_orientation_matrix_to_scalar(mGyroOrientation); |
| inv_set_gyro_orientation_and_scale(orient, mGyroScale << 15); |
| LOGI_IF(EXTRA_VERBOSE, "HAL: Set MPL Gyro Scale %ld", mGyroScale << 15); |
| |
| /* accel setup */ |
| orient = inv_orientation_matrix_to_scalar(mAccelOrientation); |
| /* use for third party accel input subsystem driver |
| inv_set_accel_orientation_and_scale(orient, 1LL << 22); |
| */ |
| inv_set_accel_orientation_and_scale(orient, (long)mAccelScale << 15); |
| LOGI_IF(EXTRA_VERBOSE, |
| "HAL: Set MPL Accel Scale %ld", (long)mAccelScale << 15); |
| |
| /* compass setup */ |
| signed char orientMtx[9]; |
| mCompassSensor->getOrientationMatrix(orientMtx); |
| orient = |
| inv_orientation_matrix_to_scalar(orientMtx); |
| long sensitivity; |
| sensitivity = mCompassSensor->getSensitivity(); |
| inv_set_compass_orientation_and_scale(orient, sensitivity); |
| mCompassScale = sensitivity; |
| LOGI_IF(EXTRA_VERBOSE, |
| "HAL: Set MPL Compass Scale %ld", mCompassScale); |
| } |
| |
| void MPLSensor::loadDMP(void) |
| { |
| VFUNC_LOG; |
| |
| int res, fd; |
| FILE *fptr; |
| |
| if (isMpuNonDmp()) { |
| return; |
| } |
| |
| /* load DMP firmware */ |
| LOGV_IF(SYSFS_VERBOSE, |
| "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp()); |
| fd = open(mpu.firmware_loaded, O_RDONLY); |
| if(fd < 0) { |
| LOGE("HAL:could not open dmp state"); |
| } else { |
| if(inv_read_dmp_state(fd) == 0) { |
| LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware); |
| fptr = fopen(mpu.dmp_firmware, "w"); |
| if(fptr == NULL) { |
| LOGE("HAL:could not open dmp_firmware"); |
| } else { |
| if (inv_load_dmp(fptr) < 0) { |
| LOGE("HAL:load DMP failed"); |
| } else { |
| LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded"); |
| } |
| if (fclose(fptr) < 0) { |
| LOGE("HAL:could not close dmp firmware"); |
| } |
| } |
| } else { |
| LOGV_IF(ENG_VERBOSE, "HAL:DMP is already loaded"); |
| } |
| } |
| |
| // onDmp(1); //Can't enable here. See note onDmp() |
| } |
| |
| void MPLSensor::inv_get_sensors_orientation(void) |
| { |
| VFUNC_LOG; |
| |
| FILE *fptr; |
| |
| // get gyro orientation |
| LOGV_IF(SYSFS_VERBOSE, |
| "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp()); |
| fptr = fopen(mpu.gyro_orient, "r"); |
| if (fptr != NULL) { |
| int om[9]; |
| if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", |
| &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], |
| &om[6], &om[7], &om[8]) < 0) { |
| LOGE("HAL:Could not read gyro mounting matrix"); |
| } else { |
| LOGV_IF(EXTRA_VERBOSE, |
| "HAL:gyro mounting matrix: " |
| "%+d %+d %+d %+d %+d %+d %+d %+d %+d", |
| om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); |
| |
| mGyroOrientation[0] = om[0]; |
| mGyroOrientation[1] = om[1]; |
| mGyroOrientation[2] = om[2]; |
| mGyroOrientation[3] = om[3]; |
| mGyroOrientation[4] = om[4]; |
| mGyroOrientation[5] = om[5]; |
| mGyroOrientation[6] = om[6]; |
| mGyroOrientation[7] = om[7]; |
| mGyroOrientation[8] = om[8]; |
| } |
| if (fclose(fptr) < 0) { |
| LOGE("HAL:Could not close gyro mounting matrix"); |
| } |
| } |
| |
| // get accel orientation |
| LOGV_IF(SYSFS_VERBOSE, |
| "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp()); |
| fptr = fopen(mpu.accel_orient, "r"); |
| if (fptr != NULL) { |
| int om[9]; |
| if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", |
| &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], |
| &om[6], &om[7], &om[8]) < 0) { |
| LOGE("HAL:could not read accel mounting matrix"); |
| } else { |
| LOGV_IF(EXTRA_VERBOSE, |
| "HAL:accel mounting matrix: " |
| "%+d %+d %+d %+d %+d %+d %+d %+d %+d", |
| om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); |
| |
| mAccelOrientation[0] = om[0]; |
| mAccelOrientation[1] = om[1]; |
| mAccelOrientation[2] = om[2]; |
| mAccelOrientation[3] = om[3]; |
| mAccelOrientation[4] = om[4]; |
| mAccelOrientation[5] = om[5]; |
| mAccelOrientation[6] = om[6]; |
| mAccelOrientation[7] = om[7]; |
| mAccelOrientation[8] = om[8]; |
| } |
| if (fclose(fptr) < 0) { |
| LOGE("HAL:could not close accel mounting matrix"); |
| } |
| } |
| } |
| |
| MPLSensor::~MPLSensor() |
| { |
| VFUNC_LOG; |
| |
| /* Close open fds */ |
| if (iio_fd > 0) |
| close(iio_fd); |
| if( accel_fd > 0 ) |
| close(accel_fd ); |
| if (gyro_temperature_fd > 0) |
| close(gyro_temperature_fd); |
| if (sysfs_names_ptr) |
| free(sysfs_names_ptr); |
| |
| closeDmpOrientFd(); |
| |
| if (accel_x_dmp_bias_fd > 0) { |
| close(accel_x_dmp_bias_fd); |
| } |
| if (accel_y_dmp_bias_fd > 0) { |
| close(accel_y_dmp_bias_fd); |
| } |
| if (accel_z_dmp_bias_fd > 0) { |
| close(accel_z_dmp_bias_fd); |
| } |
| |
| if (gyro_x_dmp_bias_fd > 0) { |
| close(gyro_x_dmp_bias_fd); |
| } |
| if (gyro_y_dmp_bias_fd > 0) { |
| close(gyro_y_dmp_bias_fd); |
| } |
| if (gyro_z_dmp_bias_fd > 0) { |
| close(gyro_z_dmp_bias_fd); |
| } |
| |
| if (gyro_x_offset_fd > 0) { |
| close(gyro_x_offset_fd); |
| } |
| if (gyro_y_offset_fd > 0) { |
| close(gyro_y_offset_fd); |
| } |
| if (gyro_z_offset_fd > 0) { |
| close(gyro_z_offset_fd); |
| } |
| |
| /* Turn off Gyro master enable */ |
| /* A workaround until driver handles it */ |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 0, mpu.master_enable, getTimestamp()); |
| write_sysfs_int(mpu.master_enable, 0); |
| |
| #ifdef INV_PLAYBACK_DBG |
| inv_turn_off_data_logging(); |
| if (fclose(logfile) < 0) { |
| LOGE("cannot close debug log file"); |
| } |
| #endif |
| } |
| |
| #define GY_ENABLED ((1 << ID_GY) & enabled_sensors) |
| #define RGY_ENABLED ((1 << ID_RG) & enabled_sensors) |
| #define A_ENABLED ((1 << ID_A) & enabled_sensors) |
| #define M_ENABLED ((1 << ID_M) & enabled_sensors) |
| #define RM_ENABLED ((1 << ID_RM) & enabled_sensors) |
| #define O_ENABLED ((1 << ID_O) & enabled_sensors) |
| #define LA_ENABLED ((1 << ID_LA) & enabled_sensors) |
| #define GR_ENABLED ((1 << ID_GR) & enabled_sensors) |
| #define RV_ENABLED ((1 << ID_RV) & enabled_sensors) |
| #define GRV_ENABLED ((1 << ID_GRV) & enabled_sensors) |
| #define GMRV_ENABLED ((1 << ID_GMRV) & enabled_sensors) |
| |
| #ifdef ENABLE_PRESSURE |
| #define PS_ENABLED ((1 << ID_PS) & enabled_sensors) |
| #endif |
| |
| /* this applies to BMA250 Input Subsystem Driver only */ |
| int MPLSensor::setAccelInitialState() |
| { |
| VFUNC_LOG; |
| |
| struct input_absinfo absinfo_x; |
| struct input_absinfo absinfo_y; |
| struct input_absinfo absinfo_z; |
| float value; |
| if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) && |
| !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) && |
| !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) { |
| value = absinfo_x.value; |
| mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X; |
| value = absinfo_y.value; |
| mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y; |
| value = absinfo_z.value; |
| mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z; |
| //mHasPendingEvent = true; |
| } |
| return 0; |
| } |
| |
| int MPLSensor::onDmp(int en) |
| { |
| VFUNC_LOG; |
| |
| int res = -1; |
| int status; |
| mDmpOn = en; |
| |
| //Sequence to enable DMP |
| //1. Load DMP image if not already loaded |
| //2. Either Gyro or Accel must be enabled/configured before next step |
| //3. Enable DMP |
| |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", |
| mpu.firmware_loaded, getTimestamp()); |
| if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){ |
| LOGE("HAL:ERR can't get firmware_loaded status"); |
| } else if (status == 1) { |
| //Write only if curr DMP state <> request |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", |
| mpu.dmp_on, getTimestamp()); |
| if (read_sysfs_int(mpu.dmp_on, &status) < 0) { |
| LOGE("HAL:ERR can't read DMP state"); |
| } else if (status != en) { |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.dmp_on, getTimestamp()); |
| if (write_sysfs_int(mpu.dmp_on, en) < 0) { |
| LOGE("HAL:ERR can't write dmp_on"); |
| } else { |
| mDmpOn = en; |
| res = 0; //Indicate write successful |
| if(!en) { |
| setAccelBias(); |
| } |
| } |
| //Enable DMP interrupt |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.dmp_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.dmp_int_on, en) < 0) { |
| LOGE("HAL:ERR can't en/dis DMP interrupt"); |
| } |
| |
| // disable DMP event interrupt if needed |
| if (!en) { |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.dmp_event_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) { |
| res = -1; |
| LOGE("HAL:ERR can't enable DMP event interrupt"); |
| } |
| } |
| } else { |
| mDmpOn = en; |
| res = 0; //DMP already set as requested |
| if(!en) { |
| setAccelBias(); |
| } |
| } |
| } else { |
| LOGE("HAL:ERR No DMP image"); |
| } |
| return res; |
| } |
| |
| int MPLSensor::setDmpFeature(int en) |
| { |
| int res = 0; |
| |
| // set sensor engine and fifo |
| 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)) { |
| res = enableGyro(1); |
| if (res < 0) { |
| return res; |
| } |
| if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) { |
| res = turnOffGyroFifo(); |
| if (res < 0) { |
| return res; |
| } |
| } |
| } |
| res = enableAccel(1); |
| if (res < 0) { |
| return res; |
| } |
| if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) { |
| res = turnOffAccelFifo(); |
| if (res < 0) { |
| return res; |
| } |
| } |
| } else { |
| if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) { |
| res = enableGyro(0); |
| if (res < 0) { |
| return res; |
| } |
| } |
| if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) { |
| res = enableAccel(0); |
| if (res < 0) { |
| return res; |
| } |
| } |
| } |
| |
| // set sensor data interrupt |
| uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE)); |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| !dataInterrupt, mpu.dmp_event_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.dmp_event_int_on, !dataInterrupt) < 0) { |
| res = -1; |
| LOGE("HAL:ERR can't enable DMP event interrupt"); |
| } |
| return res; |
| } |
| |
| int MPLSensor::computeDmpState(bool* dmp_state) |
| { |
| int res = 0; |
| bool dmpState = 0; |
| |
| if (mFeatureActiveMask) { |
| dmpState = 1; |
| LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() mFeatureActiveMask = 1"); |
| } else if ((mEnabled & VIRTUAL_SENSOR_9AXES_MASK) |
| || (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK)) { |
| if (checkLPQuaternion() && checkLPQRateSupported()) { |
| dmpState = 1; |
| LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() Sensor Fusion = 1"); |
| } |
| } /*else { |
| unsigned long sensor = mLocalSensorMask & mMasterSensorMask; |
| if (sensor & (INV_THREE_AXIS_ACCEL & INV_THREE_AXIS_GYRO)) { |
| dmpState = 1; |
| LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() accel and gyro = 1"); |
| } |
| }*/ |
| |
| *dmp_state = dmpState; |
| |
| return res; |
| } |
| |
| int MPLSensor::SetDmpState(bool dmpState) |
| { |
| int res = 0; |
| |
| // set Dmp state |
| res = onDmp(dmpState); |
| if (res < 0) |
| return res; |
| |
| if (dmpState) { |
| // set DMP rate to 200Hz |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 200, mpu.accel_fifo_rate, getTimestamp()); |
| if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) { |
| res = -1; |
| LOGE("HAL:ERR can't set rate to 200Hz"); |
| return res; |
| } |
| } |
| LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is set %s", (dmpState ? "on" : "off")); |
| mDmpState = dmpState; |
| return dmpState; |
| |
| } |
| |
| int MPLSensor::computeAndSetDmpState() |
| { |
| int res = 0; |
| bool dmpState = 0; |
| |
| computeDmpState(&dmpState); |
| |
| res = SetDmpState(dmpState); |
| if (res < 0) |
| return res; |
| |
| return dmpState; |
| } |
| |
| /* called when batch and hw sensor enabled*/ |
| int MPLSensor::enablePedIndicator(int en) |
| { |
| VFUNC_LOG; |
| |
| int res = 0; |
| if (en) { |
| if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) { |
| //Disable DMP Pedometer Interrupt |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 0, mpu.pedometer_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) { |
| LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); |
| res = -1; // indicate an err |
| return res; |
| } |
| |
| LOGV_IF(ENG_VERBOSE, "HAL:Enabling ped standalone"); |
| // enable accel engine |
| res = enableAccel(1); |
| if (res < 0) |
| return res; |
| LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask); |
| // disable accel FIFO |
| if (!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) { |
| res = turnOffAccelFifo(); |
| if (res < 0) |
| return res; |
| } |
| } |
| } else { |
| //Disable Accel if no sensor needs it |
| if (!(mFeatureActiveMask & DMP_FEATURE_MASK) |
| && (!(mLocalSensorMask & mMasterSensorMask |
| & INV_THREE_AXIS_ACCEL))) { |
| res = enableAccel(0); |
| if (res < 0) |
| return res; |
| } |
| } |
| |
| LOGV_IF(ENG_VERBOSE, "HAL:Toggling step indicator to %d", en); |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.step_indicator_on, getTimestamp()); |
| if (write_sysfs_int(mpu.step_indicator_on, en) < 0) { |
| res = -1; |
| LOGE("HAL:ERR can't write to DMP step_indicator_on"); |
| } |
| return res; |
| } |
| |
| int MPLSensor::checkPedStandaloneBatched(void) |
| { |
| VFUNC_LOG; |
| int res = 0; |
| |
| if ((mFeatureActiveMask & INV_DMP_PEDOMETER) && |
| (mBatchEnabled & (1 << StepDetector))) { |
| res = 1; |
| } else |
| res = 0; |
| |
| LOGV_IF(ENG_VERBOSE, "HAL:checkPedStandaloneBatched=%d", res); |
| return res; |
| } |
| |
| int MPLSensor::checkPedStandaloneEnabled(void) |
| { |
| VFUNC_LOG; |
| return ((mFeatureActiveMask & INV_DMP_PED_STANDALONE)? 1:0); |
| } |
| |
| /* This feature is only used in batch mode */ |
| /* Stand-alone Step Detector */ |
| int MPLSensor::enablePedStandalone(int en) |
| { |
| VFUNC_LOG; |
| |
| if (!en) { |
| enablePedStandaloneData(0); |
| mFeatureActiveMask &= ~INV_DMP_PED_STANDALONE; |
| if (mFeatureActiveMask == 0) { |
| onDmp(0); |
| } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) { |
| //Re-enable DMP Pedometer Interrupt |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 1, mpu.pedometer_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) { |
| LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); |
| return (-1); |
| } |
| //Disable data interrupt if no continuous data |
| if (mEnabled == 0) { |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 1, mpu.dmp_event_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) { |
| LOGE("HAL:ERR can't enable DMP event interrupt"); |
| return (-1); |
| } |
| } |
| } |
| LOGV_IF(ENG_VERBOSE, "HAL:Ped Standalone disabled"); |
| } else { |
| if (enablePedStandaloneData(1) < 0 || onDmp(1) < 0) { |
| LOGE("HAL:ERR can't enable Ped Standalone"); |
| } else { |
| mFeatureActiveMask |= INV_DMP_PED_STANDALONE; |
| //Disable DMP Pedometer Interrupt |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 0, mpu.pedometer_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) { |
| LOGE("HAL:ERR can't disable Android Pedometer Interrupt"); |
| return (-1); |
| } |
| //Enable Data Interrupt |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 0, mpu.dmp_event_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) { |
| LOGE("HAL:ERR can't enable DMP event interrupt"); |
| return (-1); |
| } |
| LOGV_IF(ENG_VERBOSE, "HAL:Ped Standalone enabled"); |
| } |
| } |
| return 0; |
| } |
| |
| int MPLSensor:: enablePedStandaloneData(int en) |
| { |
| VFUNC_LOG; |
| |
| int res = 0; |
| |
| // Set DMP Ped standalone |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.step_detector_on, getTimestamp()); |
| if (write_sysfs_int(mpu.step_detector_on, en) < 0) { |
| LOGE("HAL:ERR can't write DMP step_detector_on"); |
| res = -1; //Indicate an err |
| } |
| |
| // Set DMP Step indicator |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.step_indicator_on, getTimestamp()); |
| if (write_sysfs_int(mpu.step_indicator_on, en) < 0) { |
| LOGE("HAL:ERR can't write DMP step_indicator_on"); |
| res = -1; //Indicate an err |
| } |
| |
| if (!en) { |
| LOGV_IF(ENG_VERBOSE, "HAL:Disabling ped standalone"); |
| //Disable Accel if no sensor needs it |
| if (!(mFeatureActiveMask & DMP_FEATURE_MASK) |
| && (!(mLocalSensorMask & mMasterSensorMask |
| & INV_THREE_AXIS_ACCEL))) { |
| res = enableAccel(0); |
| if (res < 0) |
| return res; |
| } |
| if (!(mFeatureActiveMask & DMP_FEATURE_MASK) |
| && (!(mLocalSensorMask & mMasterSensorMask |
| & INV_THREE_AXIS_GYRO))) { |
| res = enableGyro(0); |
| if (res < 0) |
| return res; |
| } |
| } else { |
| LOGV_IF(ENG_VERBOSE, "HAL:Enabling ped standalone"); |
| // enable accel engine |
| res = enableAccel(1); |
| if (res < 0) |
| return res; |
| LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask); |
| // disable accel FIFO |
| if (!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) { |
| res = turnOffAccelFifo(); |
| if (res < 0) |
| return res; |
| } |
| } |
| |
| return res; |
| } |
| |
| int MPLSensor::checkPedQuatEnabled(void) |
| { |
| VFUNC_LOG; |
| return ((mFeatureActiveMask & INV_DMP_PED_QUATERNION)? 1:0); |
| } |
| |
| /* This feature is only used in batch mode */ |
| /* Step Detector && Game Rotation Vector */ |
| int MPLSensor::enablePedQuaternion(int en) |
| { |
| VFUNC_LOG; |
| |
| if (!en) { |
| enablePedQuaternionData(0); |
| mFeatureActiveMask &= ~INV_DMP_PED_QUATERNION; |
| if (mFeatureActiveMask == 0) { |
| onDmp(0); |
| } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) { |
| //Re-enable DMP Pedometer Interrupt |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 1, mpu.pedometer_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) { |
| LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); |
| return (-1); |
| } |
| //Disable data interrupt if no continuous data |
| if (mEnabled == 0) { |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 1, mpu.dmp_event_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) { |
| LOGE("HAL:ERR can't enable DMP event interrupt"); |
| return (-1); |
| } |
| } |
| } |
| LOGV_IF(ENG_VERBOSE, "HAL:Ped Quat disabled"); |
| } else { |
| if (enablePedQuaternionData(1) < 0 || onDmp(1) < 0) { |
| LOGE("HAL:ERR can't enable Ped Quaternion"); |
| } else { |
| mFeatureActiveMask |= INV_DMP_PED_QUATERNION; |
| //Disable DMP Pedometer Interrupt |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 0, mpu.pedometer_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) { |
| LOGE("HAL:ERR can't disable Android Pedometer Interrupt"); |
| return (-1); |
| } |
| //Enable Data Interrupt |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 0, mpu.dmp_event_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) { |
| LOGE("HAL:ERR can't enable DMP event interrupt"); |
| return (-1); |
| } |
| LOGV_IF(ENG_VERBOSE, "HAL:Ped Quat enabled"); |
| } |
| } |
| return 0; |
| } |
| |
| int MPLSensor::enablePedQuaternionData(int en) |
| { |
| VFUNC_LOG; |
| |
| int res = 0; |
| |
| // Enable DMP quaternion |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.ped_q_on, getTimestamp()); |
| if (write_sysfs_int(mpu.ped_q_on, en) < 0) { |
| LOGE("HAL:ERR can't write DMP ped_q_on"); |
| res = -1; //Indicate an err |
| } |
| |
| if (!en) { |
| LOGV_IF(ENG_VERBOSE, "HAL:Disabling ped quat"); |
| //Disable Accel if no sensor needs it |
| if (!(mFeatureActiveMask & DMP_FEATURE_MASK) |
| && (!(mLocalSensorMask & mMasterSensorMask |
| & INV_THREE_AXIS_ACCEL))) { |
| res = enableAccel(0); |
| if (res < 0) |
| return res; |
| } |
| if (!(mFeatureActiveMask & DMP_FEATURE_MASK) |
| && (!(mLocalSensorMask & mMasterSensorMask |
| & INV_THREE_AXIS_GYRO))) { |
| res = enableGyro(0); |
| if (res < 0) |
| return res; |
| } |
| if (mFeatureActiveMask & INV_DMP_QUATERNION) { |
| res = write_sysfs_int(mpu.gyro_fifo_enable, 1); |
| res += write_sysfs_int(mpu.accel_fifo_enable, 1); |
| if (res < 0) |
| return res; |
| } |
| // LOGV_IF(ENG_VERBOSE, "before mLocalSensorMask=0x%lx", mLocalSensorMask); |
| // reset global mask for buildMpuEvent() |
| if (mEnabled & (1 << GameRotationVector)) { |
| mLocalSensorMask |= INV_THREE_AXIS_GYRO; |
| mLocalSensorMask |= INV_THREE_AXIS_ACCEL; |
| } else if (mEnabled & (1 << Accelerometer)) { |
| mLocalSensorMask |= INV_THREE_AXIS_ACCEL; |
| } else if ((mEnabled & ( 1 << Gyro)) || |
| (mEnabled & (1 << RawGyro))) { |
| mLocalSensorMask |= INV_THREE_AXIS_GYRO; |
| } |
| //LOGV_IF(ENG_VERBOSE, "after mLocalSensorMask=0x%lx", mLocalSensorMask); |
| } else { |
| LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling ped quat"); |
| // enable accel engine |
| res = enableAccel(1); |
| if (res < 0) |
| return res; |
| |
| // enable gyro engine |
| res = enableGyro(1); |
| if (res < 0) |
| return res; |
| LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask); |
| // disable accel FIFO |
| if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) || |
| !(mBatchEnabled & (1 << Accelerometer))) { |
| res = turnOffAccelFifo(); |
| if (res < 0) |
| return res; |
| mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; |
| } |
| |
| // disable gyro FIFO |
| if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_GYRO)) || |
| !((mBatchEnabled & (1 << Gyro)) || (mBatchEnabled & (1 << RawGyro)))) { |
| res = turnOffGyroFifo(); |
| if (res < 0) |
| return res; |
| mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; |
| } |
| } |
| |
| return res; |
| } |
| |
| int MPLSensor::setPedQuaternionRate(int64_t wanted) |
| { |
| VFUNC_LOG; |
| int res = 0; |
| |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| int(1000000000.f / wanted), mpu.ped_q_rate, |
| getTimestamp()); |
| res = write_sysfs_int(mpu.ped_q_rate, 1000000000.f / wanted); |
| LOGV_IF(PROCESS_VERBOSE, |
| "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / wanted); |
| |
| return res; |
| } |
| |
| int MPLSensor::check6AxisQuatEnabled(void) |
| { |
| VFUNC_LOG; |
| return ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)? 1:0); |
| } |
| |
| /* This is used for batch mode only */ |
| /* GRV is batched but not along with ped */ |
| int MPLSensor::enable6AxisQuaternion(int en) |
| { |
| VFUNC_LOG; |
| |
| if (!en) { |
| enable6AxisQuaternionData(0); |
| mFeatureActiveMask &= ~INV_DMP_6AXIS_QUATERNION; |
| if (mFeatureActiveMask == 0) { |
| onDmp(0); |
| } |
| LOGV_IF(ENG_VERBOSE, "HAL:6 Axis Quat disabled"); |
| } else { |
| if (enable6AxisQuaternionData(1) < 0 || onDmp(1) < 0) { |
| LOGE("HAL:ERR can't enable 6 Axis Quaternion"); |
| } else { |
| mFeatureActiveMask |= INV_DMP_6AXIS_QUATERNION; |
| LOGV_IF(PROCESS_VERBOSE, "HAL:6 Axis Quat enabled"); |
| } |
| } |
| return 0; |
| } |
| |
| int MPLSensor::enable6AxisQuaternionData(int en) |
| { |
| VFUNC_LOG; |
| |
| int res = 0; |
| |
| // Enable DMP quaternion |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.six_axis_q_on, getTimestamp()); |
| if (write_sysfs_int(mpu.six_axis_q_on, en) < 0) { |
| LOGE("HAL:ERR can't write DMP six_axis_q_on"); |
| res = -1; //Indicate an err |
| } |
| |
| if (!en) { |
| LOGV_IF(EXTRA_VERBOSE, "HAL:DMP six axis quaternion data was turned off"); |
| inv_quaternion_sensor_was_turned_off(); |
| if (!(mFeatureActiveMask & DMP_FEATURE_MASK) |
| && (!(mLocalSensorMask & mMasterSensorMask |
| & INV_THREE_AXIS_ACCEL))) { |
| res = enableAccel(0); |
| if (res < 0) |
| return res; |
| } |
| if (!(mFeatureActiveMask & DMP_FEATURE_MASK) |
| && (!(mLocalSensorMask & mMasterSensorMask |
| & INV_THREE_AXIS_GYRO))) { |
| res = enableGyro(0); |
| if (res < 0) |
| return res; |
| } |
| if (mFeatureActiveMask & INV_DMP_QUATERNION) { |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 1, mpu.gyro_fifo_enable, getTimestamp()); |
| res = write_sysfs_int(mpu.gyro_fifo_enable, 1); |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 1, mpu.accel_fifo_enable, getTimestamp()); |
| res += write_sysfs_int(mpu.accel_fifo_enable, 1); |
| if (res < 0) |
| return res; |
| } |
| LOGV_IF(ENG_VERBOSE, " k=0x%lx", mLocalSensorMask); |
| // reset global mask for buildMpuEvent() |
| if (mEnabled & (1 << GameRotationVector)) { |
| if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) { |
| mLocalSensorMask |= INV_THREE_AXIS_GYRO; |
| mLocalSensorMask |= INV_THREE_AXIS_ACCEL; |
| res = write_sysfs_int(mpu.gyro_fifo_enable, 1); |
| res += write_sysfs_int(mpu.accel_fifo_enable, 1); |
| if (res < 0) |
| return res; |
| } |
| } else if (mEnabled & (1 << Accelerometer)) { |
| mLocalSensorMask |= INV_THREE_AXIS_ACCEL; |
| } else if ((mEnabled & ( 1 << Gyro)) || |
| (mEnabled & (1 << RawGyro))) { |
| mLocalSensorMask |= INV_THREE_AXIS_GYRO; |
| } |
| LOGV_IF(ENG_VERBOSE, "after mLocalSensorMask=0x%lx", mLocalSensorMask); |
| } else { |
| LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling six axis quat"); |
| if (mEnabled & ( 1 << GameRotationVector)) { |
| // enable accel engine |
| res = enableAccel(1); |
| if (res < 0) |
| return res; |
| |
| // enable gyro engine |
| res = enableGyro(1); |
| if (res < 0) |
| return res; |
| LOGV_IF(EXTRA_VERBOSE, "before: mLocalSensorMask=0x%lx", mLocalSensorMask); |
| if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) || |
| (!(mBatchEnabled & (1 << Accelerometer)) || |
| (!(mEnabled & (1 << Accelerometer))))) { |
| res = turnOffAccelFifo(); |
| if (res < 0) |
| return res; |
| mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; |
| } |
| |
| if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) || |
| (!(mBatchEnabled & (1 << Gyro)) || |
| (!(mEnabled & (1 << Gyro))))) { |
| if (!(mBatchEnabled & (1 << RawGyro)) || |
| (!(mEnabled & (1 << RawGyro)))) { |
| res = turnOffGyroFifo(); |
| if (res < 0) |
| return res; |
| mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; |
| } |
| } |
| LOGV_IF(ENG_VERBOSE, "after: mLocalSensorMask=0x%lx", mLocalSensorMask); |
| } |
| } |
| |
| return res; |
| } |
| |
| int MPLSensor::set6AxisQuaternionRate(int64_t wanted) |
| { |
| VFUNC_LOG; |
| int res = 0; |
| |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| int(1000000000.f / wanted), mpu.six_axis_q_rate, |
| getTimestamp()); |
| res = write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / wanted); |
| LOGV_IF(PROCESS_VERBOSE, |
| "HAL:DMP six axis rate %.2f Hz", 1000000000.f / wanted); |
| |
| return res; |
| } |
| |
| /* this is for batch mode only */ |
| int MPLSensor::checkLPQRateSupported(void) |
| { |
| VFUNC_LOG; |
| #ifndef USE_LPQ_AT_FASTEST |
| return ((mDelays[GameRotationVector] <= RATE_200HZ) ? 0 :1); |
| #else |
| return 1; |
| #endif |
| } |
| |
| int MPLSensor::checkLPQuaternion(void) |
| { |
| VFUNC_LOG; |
| return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0); |
| } |
| |
| int MPLSensor::enableLPQuaternion(int en) |
| { |
| VFUNC_LOG; |
| |
| if (!en) { |
| enableQuaternionData(0); |
| mFeatureActiveMask &= ~INV_DMP_QUATERNION; |
| if (mFeatureActiveMask == 0) { |
| onDmp(0); |
| } |
| LOGV_IF(ENG_VERBOSE, "HAL:LP Quat disabled"); |
| } else { |
| if (enableQuaternionData(1) < 0 || onDmp(1) < 0) { |
| LOGE("HAL:ERR can't enable LP Quaternion"); |
| } else { |
| mFeatureActiveMask |= INV_DMP_QUATERNION; |
| LOGV_IF(ENG_VERBOSE, "HAL:LP Quat enabled"); |
| } |
| } |
| return 0; |
| } |
| |
| int MPLSensor::enableQuaternionData(int en) |
| { |
| VFUNC_LOG; |
| |
| int res = 0; |
| |
| // Enable DMP quaternion |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.three_axis_q_on, getTimestamp()); |
| if (write_sysfs_int(mpu.three_axis_q_on, en) < 0) { |
| LOGE("HAL:ERR can't write DMP three_axis_q__on"); |
| res = -1; //Indicates an err |
| } |
| |
| if (!en) { |
| LOGV_IF(ENG_VERBOSE, "HAL:DMP quaternion data was turned off"); |
| inv_quaternion_sensor_was_turned_off(); |
| } else { |
| LOGV_IF(ENG_VERBOSE, "HAL:Enabling three axis quat"); |
| } |
| |
| return res; |
| } |
| |
| int MPLSensor::setQuaternionRate(int64_t wanted) |
| { |
| VFUNC_LOG; |
| int res = 0; |
| |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| int(1000000000.f / wanted), mpu.three_axis_q_rate, |
| getTimestamp()); |
| res = write_sysfs_int(mpu.three_axis_q_rate, 1000000000.f / wanted); |
| LOGV_IF(PROCESS_VERBOSE, |
| "HAL:DMP three axis rate %.2f Hz", 1000000000.f / wanted); |
| |
| return res; |
| } |
| |
| int MPLSensor::enableDmpPedometer(int en, int interruptMode) |
| { |
| VFUNC_LOG; |
| int res = 0; |
| int enabled_sensors = mEnabled; |
| |
| if (isMpuNonDmp()) |
| return res; |
| |
| // reset master enable |
| res = masterEnable(0); |
| if (res < 0) { |
| return res; |
| } |
| |
| if (en == 1) { |
| //Enable DMP Pedometer Function |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.pedometer_on, getTimestamp()); |
| if (write_sysfs_int(mpu.pedometer_on, en) < 0) { |
| LOGE("HAL:ERR can't enable Android Pedometer"); |
| res = -1; // indicate an err |
| return res; |
| } |
| |
| if (interruptMode) { |
| if(!checkPedStandaloneBatched()) { |
| //Enable DMP Pedometer Interrupt |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.pedometer_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) { |
| LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); |
| res = -1; // indicate an err |
| return res; |
| } |
| } |
| } |
| |
| if (interruptMode) { |
| mFeatureActiveMask |= INV_DMP_PEDOMETER; |
| } |
| else { |
| mFeatureActiveMask |= INV_DMP_PEDOMETER_STEP; |
| } |
| |
| mt_pre_ns = android::elapsedRealtimeNano(); |
| } else { |
| if (interruptMode) { |
| mFeatureActiveMask &= ~INV_DMP_PEDOMETER; |
| } |
| else { |
| mFeatureActiveMask &= ~INV_DMP_PEDOMETER_STEP; |
| mStepCountPollTime = -1; |
| } |
| |
| /* if neither step detector or step count is on */ |
| if (!(mFeatureActiveMask & (INV_DMP_PEDOMETER | INV_DMP_PEDOMETER_STEP))) { |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.pedometer_on, getTimestamp()); |
| if (write_sysfs_int(mpu.pedometer_on, en) < 0) { |
| LOGE("HAL:ERR can't enable Android Pedometer"); |
| res = -1; |
| return res; |
| } |
| } |
| |
| /* if feature is not step detector */ |
| if (!(mFeatureActiveMask & INV_DMP_PEDOMETER)) { |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.pedometer_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) { |
| LOGE("HAL:ERR can't enable Android Pedometer Interrupt"); |
| res = -1; |
| return res; |
| } |
| } |
| } |
| |
| if ((res = setDmpFeature(en)) < 0) |
| return res; |
| |
| if ((res = computeAndSetDmpState()) < 0) |
| return res; |
| |
| if (!mBatchEnabled && (resetDataRates() < 0)) |
| return res; |
| |
| if(en || enabled_sensors || mFeatureActiveMask) { |
| res = masterEnable(1); |
| } |
| return res; |
| } |
| |
| int MPLSensor::masterEnable(int en) |
| { |
| VFUNC_LOG; |
| |
| int res = 0; |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.master_enable, getTimestamp()); |
| res = write_sysfs_int(mpu.master_enable, en); |
| return res; |
| } |
| |
| int MPLSensor::enableGyro(int en) |
| { |
| VFUNC_LOG; |
| |
| int res = 0; |
| |
| /* need to also turn on/off the master enable */ |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.gyro_enable, getTimestamp()); |
| res = write_sysfs_int(mpu.gyro_enable, en); |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.gyro_fifo_enable, getTimestamp()); |
| res += write_sysfs_int(mpu.gyro_fifo_enable, en); |
| |
| if (!en) { |
| LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off"); |
| inv_gyro_was_turned_off(); |
| } |
| |
| return res; |
| } |
| |
| int MPLSensor::enableLowPowerAccel(int en) |
| { |
| VFUNC_LOG; |
| |
| int res; |
| |
| /* need to also turn on/off the master enable */ |
| res = write_sysfs_int(mpu.motion_lpa_on, en); |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.motion_lpa_on, getTimestamp()); |
| return res; |
| } |
| |
| int MPLSensor::enableAccel(int en) |
| { |
| VFUNC_LOG; |
| |
| int res; |
| |
| /* need to also turn on/off the master enable */ |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.accel_enable, getTimestamp()); |
| res = write_sysfs_int(mpu.accel_enable, en); |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| en, mpu.accel_fifo_enable, getTimestamp()); |
| res += write_sysfs_int(mpu.accel_fifo_enable, en); |
| |
| if (!en) { |
| LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off"); |
| inv_accel_was_turned_off(); |
| } |
| |
| return res; |
| } |
| |
| int MPLSensor::enableCompass(int en, int rawSensorRequested) |
| { |
| VFUNC_LOG; |
| |
| int res = 0; |
| /* TODO: handle ID_RM if third party compass cal is used */ |
| if (rawSensorRequested && mCompassSensor->providesCalibration()) { |
| res = mCompassSensor->enable(ID_RM, en); |
| } else { |
| res = mCompassSensor->enable(ID_M, en); |
| } |
| if (en == 0 || res != 0) { |
| LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off %d", res); |
| inv_compass_was_turned_off(); |
| } |
| |
| return res; |
| } |
| |
| #ifdef ENABLE_PRESSURE |
| int MPLSensor::enablePressure(int en) |
| { |
| VFUNC_LOG; |
| |
| int res = 0; |
| |
| if (mPressureSensor) |
| res = mPressureSensor->enable(ID_PS, en); |
| |
| return res; |
| } |
| #endif |
| |
| /* use this function for initialization */ |
| int MPLSensor::enableBatch(int64_t timeout) |
| { |
| VFUNC_LOG; |
| |
| int res = 0; |
| |
| res = write_sysfs_int(mpu.batchmode_timeout, timeout); |
| if (timeout == 0) { |
| res = write_sysfs_int(mpu.six_axis_q_on, 0); |
| res = write_sysfs_int(mpu.ped_q_on, 0); |
| res = write_sysfs_int(mpu.step_detector_on, 0); |
| res = write_sysfs_int(mpu.step_indicator_on, 0); |
| } |
| |
| if (timeout == 0) { |
| LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:batchmode timeout is zero"); |
| } |
| |
| return res; |
| } |
| |
| void MPLSensor::computeLocalSensorMask(int enabled_sensors) |
| { |
| VFUNC_LOG; |
| |
| do { |
| #ifdef ENABLE_PRESSURE |
| /* Invensense Pressure on secondary bus */ |
| if (PS_ENABLED) { |
| LOGV_IF(ENG_VERBOSE, "PS ENABLED"); |
| mLocalSensorMask |= INV_ONE_AXIS_PRESSURE; |
| } else { |
| LOGV_IF(ENG_VERBOSE, "PS DISABLED"); |
| mLocalSensorMask &= ~INV_ONE_AXIS_PRESSURE; |
| } |
| #else |
| LOGV_IF(ENG_VERBOSE, "PS DISABLED"); |
| mLocalSensorMask &= ~INV_ONE_AXIS_PRESSURE; |
| #endif |
| |
| if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED |
| || (GRV_ENABLED && GMRV_ENABLED)) { |
| LOGV_IF(ENG_VERBOSE, "FUSION ENABLED"); |
| mLocalSensorMask |= ALL_MPL_SENSORS_NP; |
| break; |
| } |
| |
| if (GRV_ENABLED) { |
| if (!(mFeatureActiveMask & INV_DMP_BATCH_MODE) || |
| !(mBatchEnabled & (1 << GameRotationVector))) { |
| LOGV_IF(ENG_VERBOSE, "6 Axis Fusion ENABLED"); |
| mLocalSensorMask |= INV_THREE_AXIS_GYRO; |
| mLocalSensorMask |= INV_THREE_AXIS_ACCEL; |
| } else { |
| if (GY_ENABLED || RGY_ENABLED) { |
| LOGV_IF(ENG_VERBOSE, "G ENABLED"); |
| mLocalSensorMask |= INV_THREE_AXIS_GYRO; |
| } else { |
| LOGV_IF(ENG_VERBOSE, "G DISABLED"); |
| mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; |
| } |
| if (A_ENABLED) { |
| LOGV_IF(ENG_VERBOSE, "A ENABLED"); |
| mLocalSensorMask |= INV_THREE_AXIS_ACCEL; |
| } else { |
| LOGV_IF(ENG_VERBOSE, "A DISABLED"); |
| mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; |
| } |
| } |
| /* takes care of MAG case */ |
| if (M_ENABLED || RM_ENABLED) { |
| LOGV_IF(1, "M ENABLED"); |
| mLocalSensorMask |= INV_THREE_AXIS_COMPASS; |
| } else { |
| LOGV_IF(1, "M DISABLED"); |
| mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS; |
| } |
| break; |
| } |
| |
| if (GMRV_ENABLED) { |
| LOGV_IF(ENG_VERBOSE, "6 Axis Geomagnetic Fusion ENABLED"); |
| mLocalSensorMask |= INV_THREE_AXIS_ACCEL; |
| mLocalSensorMask |= INV_THREE_AXIS_COMPASS; |
| |
| /* takes care of Gyro case */ |
| if (GY_ENABLED || RGY_ENABLED) { |
| LOGV_IF(1, "G ENABLED"); |
| mLocalSensorMask |= INV_THREE_AXIS_GYRO; |
| } else { |
| LOGV_IF(1, "G DISABLED"); |
| mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; |
| } |
| break; |
| } |
| |
| #ifdef ENABLE_PRESSURE |
| if(!A_ENABLED && !M_ENABLED && !RM_ENABLED && |
| !GRV_ENABLED && !GMRV_ENABLED && !GY_ENABLED && !RGY_ENABLED && |
| !PS_ENABLED) { |
| #else |
| if(!A_ENABLED && !M_ENABLED && !RM_ENABLED && |
| !GRV_ENABLED && !GMRV_ENABLED && !GY_ENABLED && !RGY_ENABLED) { |
| #endif |
| /* Invensense compass cal */ |
| LOGV_IF(ENG_VERBOSE, "ALL DISABLED"); |
| mLocalSensorMask = 0; |
| break; |
| } |
| |
| if (GY_ENABLED || RGY_ENABLED) { |
| LOGV_IF(ENG_VERBOSE, "G ENABLED"); |
| mLocalSensorMask |= INV_THREE_AXIS_GYRO; |
| } else { |
| LOGV_IF(ENG_VERBOSE, "G DISABLED"); |
| mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; |
| } |
| |
| if (A_ENABLED) { |
| LOGV_IF(ENG_VERBOSE, "A ENABLED"); |
| mLocalSensorMask |= INV_THREE_AXIS_ACCEL; |
| } else { |
| LOGV_IF(ENG_VERBOSE, "A DISABLED"); |
| mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; |
| } |
| |
| /* Invensense compass calibration */ |
| if (M_ENABLED || RM_ENABLED) { |
| LOGV_IF(ENG_VERBOSE, "M ENABLED"); |
| mLocalSensorMask |= INV_THREE_AXIS_COMPASS; |
| } else { |
| LOGV_IF(ENG_VERBOSE, "M DISABLED"); |
| mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS; |
| } |
| } while (0); |
| } |
| |
| int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) |
| { |
| VFUNC_LOG; |
| |
| inv_error_t res = -1; |
| int on = 1; |
| int off = 0; |
| int cal_stored = 0; |
| |
| // Sequence to enable or disable a sensor |
| // 1. reset master enable (=0) |
| // 2. enable or disable a sensor |
| // 3. set master enable (=1) |
| |
| if (isLowPowerQuatEnabled() || |
| changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) | |
| (mCompassSensor->isIntegrated() << MagneticField) | |
| #ifdef ENABLE_PRESSURE |
| (mPressureSensor->isIntegrated() << Pressure) | |
| #endif |
| (mCompassSensor->isIntegrated() << RawMagneticField))) { |
| |
| /* reset master enable */ |
| res = masterEnable(0); |
| if(res < 0) { |
| return res; |
| } |
| } |
| |
| LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", |
| (unsigned int)sensors); |
| |
| if (changed & ((1 << Gyro) | (1 << RawGyro))) { |
| LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - gyro %s", |
| (sensors & INV_THREE_AXIS_GYRO? "enable": "disable")); |
| res = enableGyro(!!(sensors & INV_THREE_AXIS_GYRO)); |
| if(res < 0) { |
| return res; |
| } |
| |
| if (!cal_stored && (!en && (changed & (1 << Gyro)))) { |
| storeCalibration(); |
| cal_stored = 1; |
| } |
| } |
| |
| if (changed & (1 << Accelerometer)) { |
| LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - accel %s", |
| (sensors & INV_THREE_AXIS_ACCEL? "enable": "disable")); |
| res = enableAccel(!!(sensors & INV_THREE_AXIS_ACCEL)); |
| if(res < 0) { |
| return res; |
| } |
| |
| if (!(sensors & INV_THREE_AXIS_ACCEL) && !cal_stored) { |
| storeCalibration(); |
| cal_stored = 1; |
| } |
| } |
| |
| if (changed & ((1 << MagneticField) | (1 << RawMagneticField))) { |
| LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - compass %s", |
| (sensors & INV_THREE_AXIS_COMPASS? "enable": "disable")); |
| res = enableCompass(!!(sensors & INV_THREE_AXIS_COMPASS), changed & (1 << RawMagneticField)); |
| if(res < 0) { |
| return res; |
| } |
| |
| if (!cal_stored && (!en && (changed & (1 << MagneticField)))) { |
| storeCalibration(); |
| cal_stored = 1; |
| } |
| } |
| |
| #ifdef ENABLE_PRESSURE |
| if (changed & (1 << Pressure)) { |
| LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - pressure %s", |
| (sensors & INV_ONE_AXIS_PRESSURE? "enable": "disable")); |
| res = enablePressure(!!(sensors & INV_ONE_AXIS_PRESSURE)); |
| if(res < 0) { |
| return res; |
| } |
| } |
| #endif |
| |
| if (isLowPowerQuatEnabled()) { |
| // Enable LP Quat |
| if ((mEnabled & VIRTUAL_SENSOR_9AXES_MASK) |
| || (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK)) { |
| LOGV_IF(ENG_VERBOSE, "HAL: 9 axis or game rot enabled"); |
| if (!(changed & ((1 << Gyro) |
| | (1 << RawGyro) |
| | (1 << Accelerometer) |
| | (mCompassSensor->isIntegrated() << MagneticField) |
| | (mCompassSensor->isIntegrated() << RawMagneticField))) |
| ) { |
| /* reset master enable */ |
| res = masterEnable(0); |
| if(res < 0) { |
| return res; |
| } |
| } |
| if (!checkLPQuaternion()) { |
| enableLPQuaternion(1); |
| } else { |
| LOGV_IF(ENG_VERBOSE, "HAL:LP Quat already enabled"); |
| } |
| } else if (checkLPQuaternion()) { |
| enableLPQuaternion(0); |
| } |
| } |
| |
| /* apply accel/gyro bias to DMP bias */ |
| /* precondition: masterEnable(0), mGyroBiasAvailable=true */ |
| /* postcondition: bias is applied upon masterEnable(1) */ |
| if(!(sensors & INV_THREE_AXIS_GYRO)) { |
| setGyroBias(); |
| } |
| if(!(sensors & INV_THREE_AXIS_ACCEL)) { |
| setAccelBias(); |
| } |
| |
| /* to batch or not to batch */ |
| int batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled); |
| /* skip setBatch if there is no need to */ |
| if(((int)mOldBatchEnabledMask != batchMode) || batchMode) { |
| setBatch(batchMode,0); |
| } |
| mOldBatchEnabledMask = batchMode; |
| |
| /* check for invn hardware sensors change */ |
| if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) | |
| (mCompassSensor->isIntegrated() << MagneticField) | |
| #ifdef ENABLE_PRESSURE |
| (mPressureSensor->isIntegrated() << Pressure) | |
| #endif |
| (mCompassSensor->isIntegrated() << RawMagneticField))) { |
| LOGV_IF(ENG_VERBOSE, |
| "HAL DEBUG: Gyro, Accel, Compass, Pressure changes"); |
| if ((checkSmdSupport() == 1) || (checkPedometerSupport() == 1) || (sensors & |
| (INV_THREE_AXIS_GYRO |
| | INV_THREE_AXIS_ACCEL |
| #ifdef ENABLE_PRESSURE |
| | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated()) |
| #endif |
| | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated())))) { |
| LOGV_IF(ENG_VERBOSE, "SMD or Hardware sensors enabled"); |
| LOGV_IF(ENG_VERBOSE, |
| "mFeatureActiveMask=0x%llx", mFeatureActiveMask); |
| LOGV_IF(ENG_VERBOSE, "HAL DEBUG: LPQ, SMD, SO enabled"); |
| // disable DMP event interrupt only (w/ data interrupt) |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 0, mpu.dmp_event_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) { |
| res = -1; |
| LOGE("HAL:ERR can't disable DMP event interrupt"); |
| return res; |
| } |
| LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=0x%llx", mFeatureActiveMask); |
| LOGV_IF(ENG_VERBOSE, "DMP_FEATURE_MASK=0x%x", DMP_FEATURE_MASK); |
| if ((mFeatureActiveMask & (long long)DMP_FEATURE_MASK) && |
| !((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) || |
| (mFeatureActiveMask & INV_DMP_PED_STANDALONE) || |
| (mFeatureActiveMask & INV_DMP_PED_QUATERNION) || |
| (mFeatureActiveMask & INV_DMP_BATCH_MODE))) { |
| // enable DMP |
| onDmp(1); |
| res = enableAccel(on); |
| if(res < 0) { |
| return res; |
| } |
| LOGV_IF(ENG_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask); |
| if (((sensors | mLocalSensorMask) & INV_THREE_AXIS_ACCEL) == 0) { |
| res = turnOffAccelFifo(); |
| } |
| if(res < 0) { |
| return res; |
| } |
| } |
| } else { // all sensors idle |
| LOGV_IF(ENG_VERBOSE, "HAL DEBUG: not SMD or Hardware sensors"); |
| if (isDmpDisplayOrientationOn() |
| && (mDmpOrientationEnabled |
| || !isDmpScreenAutoRotationEnabled())) { |
| enableDmpOrientation(1); |
| } |
| |
| if (!cal_stored) { |
| storeCalibration(); |
| cal_stored = 1; |
| } |
| } |
| } else if ((changed & |
| ((!mCompassSensor->isIntegrated()) << MagneticField) || |
| ((!mCompassSensor->isIntegrated()) << RawMagneticField)) |
| && |
| !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL |
| | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated())))) |
| ) { |
| LOGV_IF(ENG_VERBOSE, "HAL DEBUG: Gyro, Accel, Compass no change"); |
| if (!cal_stored) { |
| storeCalibration(); |
| cal_stored = 1; |
| } |
| } /*else { |
| LOGV_IF(ENG_VERBOSE, "HAL DEBUG: mEnabled"); |
| if (sensors & |
| (INV_THREE_AXIS_GYRO |
| | INV_THREE_AXIS_ACCEL |
| | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { |
| res = masterEnable(1); |
| if(res < 0) |
| return res; |
| } |
| }*/ |
| |
| if (!batchMode && (resetDataRates() < 0)) { |
| LOGE("HAL:ERR can't reset output rate back to original setting"); |
| } |
| |
| if(mFeatureActiveMask || sensors) { |
| res = masterEnable(1); |
| if(res < 0) |
| return res; |
| } |
| return res; |
| } |
| |
| /* check if batch mode should be turned on or not */ |
| int MPLSensor::computeBatchSensorMask(int enableSensors, int tempBatchSensor) |
| { |
| VFUNC_LOG; |
| int batchMode = 1; |
| mFeatureActiveMask &= ~INV_DMP_BATCH_MODE; |
| |
| LOGV_IF(ENG_VERBOSE, |
| "HAL:computeBatchSensorMask: enableSensors=%d tempBatchSensor=%d", |
| enableSensors, tempBatchSensor); |
| |
| // handle initialization case |
| if (enableSensors == 0 && tempBatchSensor == 0) |
| return 0; |
| |
| // check for possible continuous data mode |
| for(int i = 0; i <= LAST_HW_SENSOR; i++) { |
| // if any one of the hardware sensor is in continuous data mode |
| // turn off batch mode. |
| if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) { |
| LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: " |
| "hardware sensor on continuous mode:%d", i); |
| return 0; |
| } |
| if ((enableSensors & (1 << i)) && (tempBatchSensor & (1 << i))) { |
| LOGV_IF(ENG_VERBOSE, |
| "HAL:computeBatchSensorMask: hardware sensor is batch:%d", |
| i); |
| // if hardware sensor is batched, check if virtual sensor is also batched |
| if ((enableSensors & (1 << GameRotationVector)) |
| && !(tempBatchSensor & (1 << GameRotationVector))) { |
| LOGV_IF(ENG_VERBOSE, |
| "HAL:computeBatchSensorMask: but virtual sensor is not:%d", |
| i); |
| return 0; |
| } |
| } |
| } |
| |
| // if virtual sensors are on but not batched, turn off batch mode. |
| for(int i = Orientation; i < NumSensors; i++) { |
| if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) { |
| LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: " |
| "composite sensor on continuous mode:%d", i); |
| return 0; |
| } |
| } |
| |
| if ((mFeatureActiveMask & INV_DMP_PEDOMETER) && !(tempBatchSensor & (1 << StepDetector))) { |
| LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: step detector on continuous mode."); |
| return 0; |
| } |
| |
| mFeatureActiveMask |= INV_DMP_BATCH_MODE; |
| LOGV_IF(EXTRA_VERBOSE, |
| "HAL:computeBatchSensorMask: batchMode=%d, mBatchEnabled=%0x", |
| batchMode, tempBatchSensor); |
| return (batchMode && tempBatchSensor); |
| } |
| |
| /* This function is called by enable() */ |
| int MPLSensor::setBatch(int en, int toggleEnable) |
| { |
| VFUNC_LOG; |
| |
| int res = 0; |
| int64_t wanted = 1000000000LL; |
| int64_t timeout = 0; |
| int timeoutInMs = 0; |
| int featureMask = computeBatchDataOutput(); |
| |
| // reset master enable |
| if (toggleEnable == 1) { |
| res = masterEnable(0); |
| if (res < 0) { |
| return res; |
| } |
| } |
| |
| /* step detector is enabled and */ |
| /* batch mode is standalone */ |
| if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) && |
| (featureMask & INV_DMP_PED_STANDALONE)) { |
| LOGV_IF(ENG_VERBOSE, "setBatch: ID_P only = 0x%x", mBatchEnabled); |
| enablePedStandalone(1); |
| } else { |
| enablePedStandalone(0); |
| } |
| |
| /* step detector and GRV are enabled and */ |
| /* batch mode is ped q */ |
| if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) && |
| (mEnabled & (1 << GameRotationVector)) && |
| (featureMask & INV_DMP_PED_QUATERNION)) { |
| LOGV_IF(ENG_VERBOSE, "setBatch: ID_P and GRV or ALL = 0x%x", mBatchEnabled); |
| LOGV_IF(ENG_VERBOSE, "setBatch: ID_P is enabled for batching, " |
| "PED quat will be automatically enabled"); |
| enableLPQuaternion(0); |
| enablePedQuaternion(1); |
| } else if (!(featureMask & INV_DMP_PED_STANDALONE)){ |
| enablePedQuaternion(0); |
| } else { |
| enablePedQuaternion(0); |
| } |
| |
| /* step detector and hardware sensors enabled */ |
| if (en && (featureMask & INV_DMP_PED_INDICATOR) && |
| ((mEnabled) || |
| (mFeatureActiveMask & INV_DMP_PED_STANDALONE))) { |
| enablePedIndicator(1); |
| } else { |
| enablePedIndicator(0); |
| } |
| |
| /* GRV is enabled and */ |
| /* batch mode is 6axis q */ |
| if (en && (mEnabled & (1 << GameRotationVector)) && |
| (featureMask & INV_DMP_6AXIS_QUATERNION)) { |
| LOGV_IF(ENG_VERBOSE, "setBatch: GRV = 0x%x", mBatchEnabled); |
| enableLPQuaternion(0); |
| enable6AxisQuaternion(1); |
| setInitial6QuatValue(); |
| } else if (!(featureMask & INV_DMP_PED_QUATERNION)){ |
| LOGV_IF(ENG_VERBOSE, "setBatch: Toggle back to normal 6 axis"); |
| if (mEnabled & (1 << GameRotationVector)) { |
| enableLPQuaternion(checkLPQRateSupported()); |
| } |
| enable6AxisQuaternion(0); |
| } else { |
| enable6AxisQuaternion(0); |
| } |
| |
| writeBatchTimeout(en); |
| |
| if (en) { |
| // enable DMP |
| res = onDmp(1); |
| if (res < 0) { |
| return res; |
| } |
| |
| // set batch rates |
| if (setBatchDataRates() < 0) { |
| LOGE("HAL:ERR can't set batch data rates"); |
| } |
| |
| // default fifo rate to 200Hz |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| 200, mpu.gyro_fifo_rate, getTimestamp()); |
| if (write_sysfs_int(mpu.gyro_fifo_rate, 200) < 0) { |
| res = -1; |
| LOGE("HAL:ERR can't set rate to 200Hz"); |
| return res; |
| } |
| } else { |
| if (mFeatureActiveMask == 0) { |
| // disable DMP |
| res = onDmp(0); |
| if (res < 0) { |
| return res; |
| } |
| /* reset sensor rate */ |
| if (resetDataRates() < 0) { |
| LOGE("HAL:ERR can't reset output rate back to original setting"); |
| } |
| } |
| } |
| |
| // set sensor data interrupt |
| uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE)); |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", |
| !dataInterrupt, mpu.dmp_event_int_on, getTimestamp()); |
| if (write_sysfs_int(mpu.dmp_event_int_on, !dataInterrupt) < 0) { |
| res = -1; |
| LOGE("HAL:ERR can't enable DMP event interrupt"); |
| } |
| |
| if (toggleEnable == 1) { |
| if (mFeatureActiveMask || mEnabled) |
| masterEnable(1); |
| } |
| return res; |
| } |
| |
| int MPLSensor::calcBatchTimeout(int en, int64_t *out) |
| { |
| VFUNC_LOG; |
| |
| int64_t timeoutInMs = 0; |
| if (en) { |
| /* take the minimum batchmode timeout */ |
| int64_t timeout = 100000000000LL; |
| int64_t ns = 0; |
| for (int i = 0; i < NumSensors; i++) { |
| LOGV_IF(0, "mFeatureActiveMask=0x%016llx, mEnabled=0x%01x, mBatchEnabled=0x%x", |
| mFeatureActiveMask, mEnabled, mBatchEnabled); |
| if (((mEnabled & (1 << i)) && (mBatchEnabled & (1 << i))) || |
| (checkPedStandaloneBatched() && (i == StepDetector))) { |
| LOGV_IF(ENG_VERBOSE, "sensor=%d, timeout=%lld", i, mBatchTimeouts[i]); |
| ns = mBatchTimeouts[i]; |
| timeout = (ns < timeout) ? ns : timeout; |
| } |
| } |
| /* Convert ns to millisecond */ |
| timeoutInMs = timeout / 1000000; |
| } else { |
| timeoutInMs = 0; |
| } |
| |
| *out = timeoutInMs; |
| |
| return 0; |
| } |
| |
| int MPLSensor::writeBatchTimeout(int en, int64_t timeoutInMs) |
| { |
| VFUNC_LOG; |
| |
| if(mBatchTimeoutInMs != timeoutInMs) { |
| /* write required timeout to sysfs */ |
| LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %lld > %s (%lld)", |
| timeoutInMs, mpu.batchmode_timeout, getTimestamp()); |
| if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) { |
| LOGE("HAL:ERR can't write batchmode_timeout"); |
| } |
| } |
| /* remember last timeout value */ |
| mBatchTimeoutInMs = timeoutInMs; |
| |
| return 0; |
| } |
| |
| int MPLSensor::writeBatchTimeout(int en) |
| { |
| VFUNC_LOG; |
| |
| int64_t timeoutInMs = 0; |
| |
| calcBatchTimeout(en, &timeoutInMs); |
| LOGV_IF(PROCESS_VERBOSE, |
| "HAL: batch timeout set to %lld ms", timeoutInMs); |
| |
| writeBatchTimeout(en, timeoutInMs); |
| |
| return 0; |
| } |
| |
| /* Store calibration file */ |
| void MPLSensor::storeCalibration(void) |
| { |
| VFUNC_LOG; |
| |
| if(mHaveGoodMpuCal == true |
| || mAccelAccuracy >= 2 |
| || mCompassAccuracy >= 3) { |
| int res = inv_store_calibration(); |
| if (res) { |
| LOGE("HAL:Cannot store calibration on file"); |
| } else { |
| LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated"); |
| } |
| } |
| } |
| |
| /* these handlers transform mpl data into one of the Android sensor types */ |
| int MPLSensor::gyroHandler(sensors_event_t* s) |
| { |
| VHANDLER_LOG; |
| int update; |
| #if defined ANDROID_LOLLIPOP |
| update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status, |
| (inv_time_t *)(&s->timestamp)); |
| #else |
| 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; |
| } |
| |
| int MPLSensor::rawGyroHandler(sensors_event_t* s) |
| { |
| VHANDLER_LOG; |
| int update; |
| #if defined ANDROID_LOLLIPOP |
| update = inv_get_sensor_type_gyroscope_raw(s->uncalibrated_gyro.uncalib, |
| &s->gyro.status, (inv_time_t *)(&s->timestamp)); |
| #else |
| 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", |
| s->uncalibrated_gyro.bias[0], s->uncalibrated_gyro.bias[1], |
| s->uncalibrated_gyro.bias[2], s->timestamp, update); |
| } |
| s->gyro.status = SENSOR_STATUS_UNRELIABLE; |
| LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d", |
| s->uncalibrated_gyro.uncalib[0], s->uncalibrated_gyro.uncalib[1], |
| s->uncalibrated_gyro.uncalib[2], s->timestamp, update); |
| return update; |
| } |
| |
| int MPLSensor::accelHandler(sensors_event_t* s) |
| { |
| VHANDLER_LOG; |
| int update; |
| #if defined ANDROID_LOLLIPOP |
| update = inv_get_sensor_type_accelerometer( |
| s->acceleration.v, &s->acceleration.status, (inv_time_t *)(&s->timestamp)); |
| #else |
| 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); |
| mAccelAccuracy = s->acceleration.status; |
| return update; |
| } |
| |
| 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)); |
| #else |
| 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 | 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()) { |
| update = mCompassSensor->readRawSample(s->uncalibrated_magnetic.uncalib, (int64_t *)(&s->timestamp)); |
| } |
| else { |
| update = inv_get_sensor_type_magnetic_field_raw(s->uncalibrated_magnetic.uncalib, |
| &s->magnetic.status, (inv_time_t *)(&s->timestamp)); |
| } |
| #else |
| if(mCompassSensor->providesCalibration()) { |
| update = mCompassSensor->readRawSample(s->uncalibrated_magnetic.uncalib, &s->timestamp); |
| } |
| else { |
| update = inv_get_sensor_type_magnetic_field_raw(s->uncalibrated_magnetic.uncalib, |
| &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", |
| s->uncalibrated_magnetic.bias[0], s->uncalibrated_magnetic.bias[1], |
| s->uncalibrated_magnetic.bias[2], s->timestamp, update); |
| } |
| s->magnetic.status = SENSOR_STATUS_UNRELIABLE; |
| 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 | overflow; |
| } |
| |
| /* |
| Rotation Vector handler. |
| NOTE: rotation vector does not have an accuracy or status |
| */ |
| int MPLSensor::rvHandler(sensors_event_t* s) |
| { |
| VHANDLER_LOG; |
| int8_t status; |
| int update; |
| #if defined ANDROID_LOLLIPOP |
| update = inv_get_sensor_type_rotation_vector(s->data, &status, |
| (inv_time_t *)(&s->timestamp)); |
| #else |
| update = inv_get_sensor_type_rotation_vector(s->data, &status, |
| &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); |
| |
| return update; |
| } |
| |
| /* |
| Game Rotation Vector handler. |
| NOTE: rotation vector does not have an accuracy or status |
| */ |
| int MPLSensor::grvHandler(sensors_event_t* s) |
| { |
| VHANDLER_LOG; |
| int8_t status; |
| int update; |
| #if defined ANDROID_LOLLIPOP |
| update = inv_get_sensor_type_rotation_vector_6_axis(s->data, &status, |
| (inv_time_t *)(&s->timestamp)); |
| #else |
| update = inv_get_sensor_type_rotation_vector_6_axis(s->data, &status, |
| &s->timestamp); |
| #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); |
| return update; |
| } |
| |
| int MPLSensor::laHandler(sensors_event_t* s) |
| { |
| VHANDLER_LOG; |
| int update; |
| #if defined ANDROID_LOLLIPOP |
| update = inv_get_sensor_type_linear_acceleration( |
| s->gyro.v, &s->gyro.status, (inv_time_t *)(&s->timestamp)); |
| #else |
| update = inv_get_sensor_type_linear_acceleration( |
| 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; |
| } |
| |
| int MPLSensor::gravHandler(sensors_event_t* s) |
| { |
| VHANDLER_LOG; |
| int update; |
| #if defined ANDROID_LOLLIPOP |
| update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status, |
| (inv_time_t *)(&s->timestamp)); |
| #else |
| update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status, |
| &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; |
| } |
| |
| int MPLSensor::orienHandler(sensors_event_t* s) |
| { |
| VHANDLER_LOG; |
| int update; |
| #if defined ANDROID_LOLLIPOP |
| update = inv_get_sensor_type_orientation( |
| s->orientation.v, &s->orientation.status, (inv_time_t *)(&s->timestamp)); |
| #else |
| update = inv_get_sensor_type_orientation( |
| s->orientation.v, &s->orientation.status, &s->timestamp); |
| |
| #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); |
| return update; |
| } |
| |
| int MPLSensor::smHandler(sensors_event_t* s) |
| { |
| VHANDLER_LOG; |
|
