blob: 5a57d0b7c6fc3ffaf58f8d02205da93f6b6a2f80 [file] [log] [blame]
/*
* 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/log.h>
#include <utils/KeyedVector.h>
#include <utils/Vector.h>
#include <utils/String8.h>
#include <string.h>
#include <linux/input.h>
#include <utils/Atomic.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(gyroBuf, 0, sizeof(gyroBuf));
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(buf, 0, sizeof(buf));
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);
}
#if 1
/* 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);
#endif
#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(mPendingEvents));
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_dmp_bias_fd);
}
if (gyro_y_offset_fd > 0) {
close(gyro_y_offset_fd);
}
if (gyro_z_offset_fd > 0) {
close(accel_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 <= GeomagneticRotationVector; 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("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