C++程序
|
7112行
|
240.84 KB
/*
* 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(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;
int update = 1;
/* When event is triggered, set data to 1 */
s->data[0] = 1.f;
s->data[1] = 0.f;
s->data[2] = 0.f;
/* Capture timestamp in HAL */
s->timestamp = android::elapsedRealtimeNano();
if (!mEnabledTime[SignificantMotion] || !(s->timestamp > mEnabledTime[SignificantMotion])) {
LOGV_IF(ENG_VERBOSE, "HAL:sm incorrect timestamp Enabled=%lld, Timestamp=%lld",
mEnabledTime[SignificantMotion], s->timestamp);
update = 0;
}
LOGV_IF(HANDLER_DATA, "HAL:sm data: %f - %lld - %d",
s->data[0], s->timestamp, update);
return update;
}
int MPLSensor::gmHandler(sensors_event_t* s)
{
VHANDLER_LOG;
int8_t status;
int update = 0;
#if defined ANDROID_LOLLIPOP
update = inv_get_sensor_type_geomagnetic_rotation_vector(s->data, &status,
(inv_time_t *)(&s->timestamp));
#else
update = inv_get_sensor_type_geomagnetic_rotation_vector(s->data, &status,
&s->timestamp);
#endif
s->orientation.status = status;
if (!mEnabledTime[GeomagneticRotationVector] || !(s->timestamp > mEnabledTime[GeomagneticRotationVector])) {
LOGV_IF(ENG_VERBOSE, "HAL:gm incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
mEnabledTime[GeomagneticRotationVector], s->timestamp, android::elapsedRealtimeNano());
update = 0;
}
LOGV_IF(HANDLER_DATA, "HAL:gm data: %+f %+f %+f %+f %+f %d- %+lld - %d",
s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->orientation.status, s->timestamp, update);
return update < 1 ? 0 :1;
}
int MPLSensor::psHandler(sensors_event_t* s)
{
VHANDLER_LOG;
int8_t status;
int update = 0;
s->pressure = mCachedPressureData / 100.f; //hpa (millibar)
s->data[1] = 0;
s->data[2] = 0;
s->timestamp = mPressureTimestamp;
s->magnetic.status = 0;
update = mPressureUpdate;
mPressureUpdate = 0;
#ifdef ENABLE_PRESSURE
if (!mEnabledTime[Pressure] || !(s->timestamp > mEnabledTime[Pressure])) {
LOGV_IF(ENG_VERBOSE, "HAL:ps incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
mEnabledTime[Pressure], s->timestamp, android::elapsedRealtimeNano());
update = 0;
}
#endif
LOGV_IF(HANDLER_DATA, "HAL:ps data: %+f %+f %+f %+f- %+lld - %d",
s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
return update < 1 ? 0 :1;
}
int MPLSensor::sdHandler(sensors_event_t* s)
{
VHANDLER_LOG;
int update = 1;
/* When event is triggered, set data to 1 */
s->data[0] = 1;
s->data[1] = 0.f;
s->data[2] = 0.f;
/* get current timestamp */
s->timestamp = android::elapsedRealtimeNano();
LOGV_IF(HANDLER_DATA, "HAL:sd data: %f - %lld - %d",
s->data[0], s->timestamp, update);
return update;
}
int MPLSensor::scHandler(sensors_event_t* s)
{
VHANDLER_LOG;
int update = 1;
/* Set step count */
#if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP
s->u64.step_counter = mLastStepCount;
LOGV_IF(HANDLER_DATA, "HAL:sc data: %lld - %lld - %d",
s->u64.step_counter, s->timestamp, update);
#else
s->step_counter = mLastStepCount;
LOGV_IF(HANDLER_DATA, "HAL:sc data: %lld - %lld - %d",
s->step_counter, s->timestamp, update);
#endif
if (s->timestamp == 0 && update) {
s->timestamp = android::elapsedRealtimeNano();
}
return update;
}
int MPLSensor::metaHandler(sensors_event_t* s, int flags)
{
VHANDLER_LOG;
int update = 1;
#if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP
/* initalize SENSOR_TYPE_META_DATA */
s->version = 0;
s->sensor = 0;
s->reserved0 = 0;
s->timestamp = 0LL;
switch(flags) {
case META_DATA_FLUSH_COMPLETE:
s->type = SENSOR_TYPE_META_DATA;
s->version = META_DATA_VERSION;
s->meta_data.what = flags;
s->meta_data.sensor = mFlushSensorEnabledVector[0];
mFlushSensorEnabledVector.removeAt(0);
LOGV_IF(HANDLER_DATA,
"HAL:flush complete data: type=%d what=%d, "
"sensor=%d - %lld - %d",
s->type, s->meta_data.what, s->meta_data.sensor,
s->timestamp, update);
break;
default:
LOGW("HAL: Meta flags not supported");
break;
}
#endif
return 1;
}
int MPLSensor::enable(int32_t handle, int en)
{
VFUNC_LOG;
android::String8 sname;
int what = -1, err = 0;
int batchMode = 0;
if (uint32_t(handle) >= NumSensors)
return -EINVAL;
/* set called flag */
mEnableCalled = 1;
if (!en)
mBatchEnabled &= ~(1 << handle);
LOGV_IF(ENG_VERBOSE, "HAL: MPLSensor::enable(handle = %d, en = %d)", handle, en);
switch (handle) {
case ID_SC:
what = StepCounter;
sname = "Step Counter";
LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
sname.string(), handle,
(mDmpStepCountEnabled? "en": "dis"),
(en? "en" : "dis"));
enableDmpPedometer(en, 0);
mDmpStepCountEnabled = !!en;
if (en)
mEnabledTime[StepCounter] = android::elapsedRealtimeNano();
else
mEnabledTime[StepCounter] = 0;
if (!en)
mBatchDelays[what] = 1000000000LL;
return 0;
case ID_P:
what = StepDetector;
sname = "StepDetector";
LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
sname.string(), handle,
(mDmpPedometerEnabled? "en": "dis"),
(en? "en" : "dis"));
enableDmpPedometer(en, 1);
mDmpPedometerEnabled = !!en;
batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled);
/* skip setBatch if there is no need to */
if(((int)mOldBatchEnabledMask != batchMode) || batchMode) {
setBatch(batchMode,1);
}
mOldBatchEnabledMask = batchMode;
if (en)
mEnabledTime[StepDetector] = android::elapsedRealtimeNano();
else
mEnabledTime[StepDetector] = 0;
if (!en)
mBatchDelays[what] = 1000000000LL;
return 0;
case ID_SM:
sname = "Significant Motion";
LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
sname.string(), handle,
(mDmpSignificantMotionEnabled? "en": "dis"),
(en? "en" : "dis"));
enableDmpSignificantMotion(en);
mDmpSignificantMotionEnabled = !!en;
if (en)
mEnabledTime[SignificantMotion] = android::elapsedRealtimeNano();
else
mEnabledTime[SignificantMotion] = 0;
return 0;
case ID_SO:
sname = "Screen Orientation";
LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
sname.string(), handle,
(mDmpOrientationEnabled? "en": "dis"),
(en? "en" : "dis"));
enableDmpOrientation(en && isDmpDisplayOrientationOn());
mDmpOrientationEnabled = !!en;
return 0;
case ID_A:
what = Accelerometer;
sname = "Accelerometer";
break;
case ID_M:
what = MagneticField;
sname = "MagneticField";
break;
case ID_RM:
what = RawMagneticField;
sname = "MagneticField Uncalibrated";
break;
case ID_O:
what = Orientation;
sname = "Orientation";
break;
case ID_GY:
what = Gyro;
sname = "Gyro";
break;
case ID_RG:
what = RawGyro;
sname = "Gyro Uncalibrated";
break;
case ID_GR:
what = Gravity;
sname = "Gravity";
break;
case ID_RV:
what = RotationVector;
sname = "RotationVector";
break;
case ID_GRV:
what = GameRotationVector;
sname = "GameRotationVector";
break;
case ID_LA:
what = LinearAccel;
sname = "LinearAccel";
break;
case ID_GMRV:
what = GeomagneticRotationVector;
sname = "GeomagneticRotationVector";
break;
#ifdef ENABLE_PRESSURE
case ID_PS:
what = Pressure;
sname = "Pressure";
break;
#endif
default:
what = handle;
sname = "Others";
break;
}
int newState = en ? 1 : 0;
unsigned long sen_mask;
LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
sname.string(), handle,
((mEnabled & (1 << what)) ? "en" : "dis"),
((uint32_t(newState) << what) ? "en" : "dis"));
LOGV_IF(ENG_VERBOSE,
"HAL:%s sensor state change what=%d", sname.string(), what);
// pthread_mutex_lock(&mMplMutex);
// pthread_mutex_lock(&mHALMutex);
if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
uint32_t sensor_type;
short flags = newState;
uint32_t lastEnabled = mEnabled, changed = 0;
mEnabled &= ~(1 << what);
mEnabled |= (uint32_t(flags) << what);
if (lastEnabled > mEnabled) {
mEnabledCached = mEnabled;
} else {
mEnabledCached = lastEnabled;
}
LOGV_IF(ENG_VERBOSE, "HAL:flags = %d", flags);
computeLocalSensorMask(mEnabled);
LOGV_IF(ENG_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
LOGV_IF(ENG_VERBOSE, "HAL:last enable : lastEnabled = %d", lastEnabled);
sen_mask = mLocalSensorMask & mMasterSensorMask;
mSensorMask = sen_mask;
LOGV_IF(ENG_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
switch (what) {
case Gyro:
case RawGyro:
case Accelerometer:
if ((!(mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK) &&
!(mEnabled & VIRTUAL_SENSOR_9AXES_MASK)) &&
((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
changed |= (1 << what);
}
if (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) {
changed |= (1 << what);
}
break;
case MagneticField:
case RawMagneticField:
if (!(mEnabled & VIRTUAL_SENSOR_9AXES_MASK) &&
((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
changed |= (1 << what);
}
break;
#ifdef ENABLE_PRESSURE
case Pressure:
if ((lastEnabled & (1 << what)) != (mEnabled & (1 << what))) {
changed |= (1 << what);
}
break;
#endif
case GameRotationVector:
if (!en)
storeCalibration();
if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK))
||
(en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
||
(!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK))
||
(!en && (mEnabled & VIRTUAL_SENSOR_MAG_6AXES_MASK))) {
for (int i = Gyro; i <= RawMagneticField; i++) {
if (!(mEnabled & (1 << i))) {
changed |= (1 << i);
}
}
}
break;
case Orientation:
case RotationVector:
case LinearAccel:
case Gravity:
if (!en)
storeCalibration();
if ((en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
||
(!en && !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK))) {
for (int i = Gyro; i <= RawMagneticField; i++) {
if (!(mEnabled & (1 << i))) {
changed |= (1 << i);
}
}
}
break;
case GeomagneticRotationVector:
if (!en)
storeCalibration();
if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK))
||
(en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
||
(!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK))
||
(!en && (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK))) {
for (int i = Accelerometer; i <= RawMagneticField; i++) {
if (!(mEnabled & (1<<i))) {
changed |= (1 << i);
}
}
}
break;
}
LOGV_IF(ENG_VERBOSE, "HAL:changed = %d", changed);
enableSensors(sen_mask, flags, changed);
// update mEnabledCached afer all configurations done
mEnabledCached = mEnabled;
if (en)
mEnabledTime[what] = android::elapsedRealtimeNano();
else
mEnabledTime[what] = 0;
if (!en)
mBatchDelays[what] = 1000000000LL;
}
// pthread_mutex_unlock(&mMplMutex);
// pthread_mutex_unlock(&mHALMutex);
#ifdef INV_PLAYBACK_DBG
/* apparently the logging needs to go through this sequence
to properly flush the log file */
inv_turn_off_data_logging();
if (fclose(logfile) < 0) {
LOGE("cannot close debug log file");
}
logfile = fopen("/data/playback.bin", "ab");
if (logfile)
inv_turn_on_data_logging(logfile);
#endif
return err;
}
void MPLSensor::getHandle(int32_t handle, int &what, android::String8 &sname)
{
VFUNC_LOG;
what = -1;
switch (handle) {
case ID_P:
what = StepDetector;
sname = "StepDetector";
break;
case ID_SC:
what = StepCounter;
sname = "StepCounter";
break;
case ID_SM:
what = SignificantMotion;
sname = "SignificantMotion";
break;
case ID_SO:
what = handle;
sname = "ScreenOrienation";
break;
case ID_A:
what = Accelerometer;
sname = "Accelerometer";
break;
case ID_M:
what = MagneticField;
sname = "MagneticField";
break;
case ID_RM:
what = RawMagneticField;
sname = "MagneticField Uncalibrated";
break;
case ID_O:
what = Orientation;
sname = "Orientation";
break;
case ID_GY:
what = Gyro;
sname = "Gyro";
break;
case ID_RG:
what = RawGyro;
sname = "Gyro Uncalibrated";
break;
case ID_GR:
what = Gravity;
sname = "Gravity";
break;
case ID_RV:
what = RotationVector;
sname = "RotationVector";
break;
case ID_GRV:
what = GameRotationVector;
sname = "GameRotationVector";
break;
case ID_LA:
what = LinearAccel;
sname = "LinearAccel";
break;
#ifdef ENABLE_PRESSURE
case ID_PS:
what = Pressure;
sname = "Pressure";
break;
#endif
default: // this takes care of all the gestures
what = handle;
sname = "Others";
break;
}
LOGI_IF(EXTRA_VERBOSE, "HAL:getHandle - what=%d, sname=%s", what, sname.string());
return;
}
int MPLSensor::setDelay(int32_t handle, int64_t ns)
{
VFUNC_LOG;
android::String8 sname;
int what = -1;
#if 0
// skip the 1st call for enalbing sensors called by ICS/JB sensor service
static int counter_delay = 0;
if (!(mEnabled & (1 << what))) {
counter_delay = 0;
} else {
if (++counter_delay == 1) {
return 0;
}
else {
counter_delay = 0;
}
}
#endif
getHandle(handle, what, sname);
if (uint32_t(what) >= NumSensors)
return -EINVAL;
if (ns < 0)
return -EINVAL;
LOGV_IF(PROCESS_VERBOSE,
"setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
// limit all rates to reasonable ones */
if (ns < 5000000LL) {
ns = 5000000LL;
}
/* store request rate to mDelays arrary for each sensor */
int64_t previousDelay = mDelays[what];
mDelays[what] = ns;
LOGV_IF(ENG_VERBOSE, "storing mDelays[%d] = %lld, previousDelay = %lld", what, ns, previousDelay);
switch (what) {
case StepCounter:
/* set limits of delivery rate of events */
mStepCountPollTime = ns;
LOGV_IF(ENG_VERBOSE, "step count rate =%lld ns", ns);
break;
case StepDetector:
case SignificantMotion:
#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
case ID_SO:
#endif
LOGV_IF(ENG_VERBOSE, "Step Detect, SMD, SO rate=%lld ns", ns);
break;
case Gyro:
case RawGyro:
case Accelerometer:
// need to update delay since they are different
// resetDataRates was called earlier
// LOGV("what=%d mEnabled=%d mDelays[%d]=%lld previousDelay=%lld",
// what, mEnabled, what, mDelays[what], previousDelay);
if ((mEnabled & (1 << what)) && (previousDelay != mDelays[what])) {
LOGV_IF(ENG_VERBOSE,
"HAL:need to update delay due to resetDataRates");
break;
}
for (int i = Gyro;
i <= Accelerometer + mCompassSensor->isIntegrated();
i++) {
if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
LOGV_IF(ENG_VERBOSE,
"HAL:ignore delay set due to sensor %d", i);
return 0;
}
}
break;
case MagneticField:
case RawMagneticField:
// need to update delay since they are different
// resetDataRates was called earlier
if ((mEnabled & (1 << what)) && (previousDelay != mDelays[what])) {
LOGV_IF(ENG_VERBOSE,
"HAL:need to update delay due to resetDataRates");
break;
}
if (mCompassSensor->isIntegrated() &&
(((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
((mEnabled & (1 << Accelerometer)) &&
ns > mDelays[Accelerometer])) &&
!checkBatchEnabled()) {
/* if request is slower rate, ignore request */
LOGV_IF(ENG_VERBOSE,
"HAL:ignore delay set due to gyro/accel");
return 0;
}
break;
case Orientation:
case RotationVector:
case GameRotationVector:
case GeomagneticRotationVector:
case LinearAccel:
case Gravity:
if (isLowPowerQuatEnabled()) {
LOGV_IF(ENG_VERBOSE,
"HAL:need to update delay due to LPQ");
break;
}
for (int i = 0; i < NumSensors; i++) {
if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
LOGV_IF(ENG_VERBOSE,
"HAL:ignore delay set due to sensor %d", i);
return 0;
}
}
break;
}
// pthread_mutex_lock(&mHALMutex);
int res = update_delay();
// pthread_mutex_unlock(&mHALMutex);
return res;
}
int MPLSensor::update_delay(void)
{
VFUNC_LOG;
int res = 0;
int64_t got;
if (mEnabled) {
int64_t wanted = 1000000000LL;
int64_t wanted_3rd_party_sensor = 1000000000LL;
// Sequence to change sensor's FIFO rate
// 1. reset master enable
// 2. Update delay
// 3. set master enable
// reset master enable
masterEnable(0);
int64_t gyroRate;
int64_t accelRate;
int64_t compassRate;
#ifdef ENABLE_PRESSURE
int64_t pressureRate;
#endif
int rateInus;
int mplGyroRate;
int mplAccelRate;
int mplCompassRate;
int tempRate = wanted;
/* search the minimum delay requested across all enabled sensors */
for (int i = 0; i < NumSensors; i++) {
if (mEnabled & (1 << i)) {
int64_t ns = mDelays[i];
wanted = wanted < ns ? wanted : ns;
}
}
/* initialize rate for each sensor */
gyroRate = wanted;
accelRate = wanted;
compassRate = wanted;
#ifdef ENABLE_PRESSURE
pressureRate = wanted;
#endif
// same delay for 3rd party Accel or Compass
wanted_3rd_party_sensor = wanted;
int enabled_sensors = mEnabled;
int tempFd = -1;
if(mFeatureActiveMask & INV_DMP_BATCH_MODE) {
// set batch rates
LOGV_IF(ENG_VERBOSE, "HAL: mFeatureActiveMask=%016llx", mFeatureActiveMask);
LOGV("HAL: batch mode is set, set batch data rates");
if (setBatchDataRates() < 0) {
LOGE("HAL:ERR can't set batch data rates");
}
} else {
/* set master sampling frequency */
int64_t tempWanted = wanted;
getDmpRate(&tempWanted);
/* driver only looks at sampling frequency if DMP is off */
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / tempWanted, mpu.gyro_fifo_rate, getTimestamp());
tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
res = write_attribute_sensor(tempFd, 1000000000.f / tempWanted);
LOGE_IF(res < 0, "HAL:sampling frequency update delay error");
if (LA_ENABLED || GR_ENABLED || RV_ENABLED
|| GRV_ENABLED || O_ENABLED || GMRV_ENABLED) {
rateInus = (int)wanted / 1000LL;
/* set rate in MPL */
/* compass can only do 100Hz max */
inv_set_gyro_sample_rate(rateInus);
inv_set_accel_sample_rate(rateInus);
inv_set_compass_sample_rate(rateInus);
inv_set_linear_acceleration_sample_rate(rateInus);
inv_set_orientation_sample_rate(rateInus);
inv_set_rotation_vector_sample_rate(rateInus);
inv_set_gravity_sample_rate(rateInus);
inv_set_orientation_geomagnetic_sample_rate(rateInus);
inv_set_rotation_vector_6_axis_sample_rate(rateInus);
inv_set_geomagnetic_rotation_vector_sample_rate(rateInus);
LOGV_IF(ENG_VERBOSE,
"HAL:MPL virtual sensor sample rate: (mpl)=%d us (mpu)=%.2f Hz",
rateInus, 1000000000.f / wanted);
LOGV_IF(ENG_VERBOSE,
"HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz",
rateInus, 1000000000.f / gyroRate);
LOGV_IF(ENG_VERBOSE,
"HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz",
rateInus, 1000000000.f / accelRate);
LOGV_IF(ENG_VERBOSE,
"HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz",
rateInus, 1000000000.f / compassRate);
LOGV_IF(ENG_VERBOSE,
"mFeatureActiveMask=%016llx", mFeatureActiveMask);
if(mFeatureActiveMask & DMP_FEATURE_MASK) {
bool setDMPrate= 0;
gyroRate = wanted;
accelRate = wanted;
compassRate = wanted;
// same delay for 3rd party Accel or Compass
wanted_3rd_party_sensor = wanted;
rateInus = (int)wanted / 1000LL;
// Set LP Quaternion sample rate if enabled
if (checkLPQuaternion()) {
if (wanted <= RATE_200HZ) {
#ifndef USE_LPQ_AT_FASTEST
enableLPQuaternion(0);
#endif
} else {
inv_set_quat_sample_rate(rateInus);
LOGV_IF(ENG_VERBOSE, "HAL:MPL quat sample rate: "
"(mpl)=%d us (mpu)=%.2f Hz",
rateInus, 1000000000.f / wanted);
setDMPrate= 1;
}
}
}
LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
//nsToHz
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / gyroRate, mpu.gyro_rate,
getTimestamp());
tempFd = open(mpu.gyro_rate, O_RDWR);
res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
if(res < 0) {
LOGE("HAL:GYRO update delay error");
}
if(USE_THIRD_PARTY_ACCEL == 1) {
// 3rd party accelerometer - if applicable
// nsToHz (BMA250)
LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
wanted_3rd_party_sensor / 1000000L, mpu.accel_rate,
getTimestamp());
tempFd = open(mpu.accel_rate, O_RDWR);
res = write_attribute_sensor(tempFd,
wanted_3rd_party_sensor / 1000000L);
LOGE_IF(res < 0, "HAL:ACCEL update delay error");
} else {
// mpu accel
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / accelRate, mpu.accel_rate,
getTimestamp());
tempFd = open(mpu.accel_rate, O_RDWR);
res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
LOGE_IF(res < 0, "HAL:ACCEL update delay error");
}
if (!mCompassSensor->isIntegrated()) {
// stand-alone compass - if applicable
LOGV_IF(ENG_VERBOSE,
"HAL:Ext compass delay %lld", wanted_3rd_party_sensor);
LOGV_IF(ENG_VERBOSE, "HAL:Ext compass rate %.2f Hz",
1000000000.f / wanted_3rd_party_sensor);
if (wanted_3rd_party_sensor <
mCompassSensor->getMinDelay() * 1000LL) {
wanted_3rd_party_sensor =
mCompassSensor->getMinDelay() * 1000LL;
}
LOGV_IF(ENG_VERBOSE,
"HAL:Ext compass delay %lld", wanted_3rd_party_sensor);
LOGV_IF(ENG_VERBOSE, "HAL:Ext compass rate %.2f Hz",
1000000000.f / wanted_3rd_party_sensor);
mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
got = mCompassSensor->getDelay(ID_M);
inv_set_compass_sample_rate(got / 1000);
} else {
// compass on secondary bus
if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
compassRate = mCompassSensor->getMinDelay() * 1000LL;
}
mCompassSensor->setDelay(ID_M, compassRate);
}
} else {
if (GY_ENABLED || RGY_ENABLED) {
wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
(mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
(mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
inv_set_gyro_sample_rate((int)wanted/1000LL);
LOGV_IF(ENG_VERBOSE,
"HAL:MPL gyro sample rate: (mpl)=%d us", int(wanted/1000LL));
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / wanted, mpu.gyro_rate, getTimestamp());
tempFd = open(mpu.gyro_rate, O_RDWR);
res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
LOGE_IF(res < 0, "HAL:GYRO update delay error");
}
if (A_ENABLED) { /* there is only 1 fifo rate for MPUxxxx */
if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
wanted = mDelays[Gyro];
} else if (RGY_ENABLED && mDelays[RawGyro]
< mDelays[Accelerometer]) {
wanted = mDelays[RawGyro];
} else {
wanted = mDelays[Accelerometer];
}
LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
inv_set_accel_sample_rate((int)wanted/1000LL);
LOGV_IF(ENG_VERBOSE, "HAL:MPL accel sample rate: (mpl)=%d us",
int(wanted/1000LL));
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / wanted, mpu.accel_rate,
getTimestamp());
tempFd = open(mpu.accel_rate, O_RDWR);
if(USE_THIRD_PARTY_ACCEL == 1) {
//BMA250 in ms
res = write_attribute_sensor(tempFd, wanted / 1000000L);
}
else {
//MPUxxxx in hz
res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
}
LOGE_IF(res < 0, "HAL:ACCEL update delay error");
}
/* Invensense compass calibration */
if (M_ENABLED || RM_ENABLED) {
int64_t compassWanted = (mDelays[MagneticField] <= mDelays[RawMagneticField]?
(mEnabled & (1 << MagneticField)? mDelays[MagneticField]: mDelays[RawMagneticField]):
(mEnabled & (1 << RawMagneticField)? mDelays[RawMagneticField]: mDelays[MagneticField]));
if (!mCompassSensor->isIntegrated()) {
wanted = compassWanted;
} else {
if (GY_ENABLED
&& (mDelays[Gyro] < compassWanted)) {
wanted = mDelays[Gyro];
} else if (RGY_ENABLED
&& mDelays[RawGyro] < compassWanted) {
wanted = mDelays[RawGyro];
} else if (A_ENABLED && mDelays[Accelerometer]
< compassWanted) {
wanted = mDelays[Accelerometer];
} else {
wanted = compassWanted;
}
LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
}
mCompassSensor->setDelay(ID_M, wanted);
got = mCompassSensor->getDelay(ID_M);
inv_set_compass_sample_rate(got / 1000);
LOGV_IF(ENG_VERBOSE, "HAL:MPL compass sample rate: (mpl)=%d us",
int(got/1000LL));
}
#ifdef ENABLE_PRESSURE
if (PS_ENABLED) {
int64_t pressureRate = mDelays[Pressure];
LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
mPressureSensor->setDelay(ID_PS, pressureRate);
LOGE_IF(res < 0, "HAL:PRESSURE update delay error");
}
#endif
}
} //end of non batch mode
unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
if (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, "sensors=%lu", sensors);
res = masterEnable(1);
if(res < 0)
return res;
} else { // all sensors idle -> reduce power, unless DMP is needed
LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
if(mFeatureActiveMask & DMP_FEATURE_MASK) {
res = masterEnable(1);
if(res < 0)
return res;
}
}
}
return res;
}
/**
* Should be called after reading at least one of gyro
* compass or accel data. (Also okay for handling all of them).
* @returns 0, if successful, error number if not.
*/
int MPLSensor::readEvents(sensors_event_t* data, int count)
{
VHANDLER_LOG;
if (!mSkipExecuteOnData)
inv_execute_on_data();
int numEventReceived = 0;
long msg;
if (count <= 0)
return 0;
msg = inv_get_message_level_0(1);
if (msg) {
if (msg & INV_MSG_MOTION_EVENT) {
LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n");
}
if (msg & INV_MSG_NO_MOTION_EVENT) {
LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n");
/* after the first no motion, the gyro should be
calibrated well */
mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
/* if gyros are on and we got a no motion, set a flag
indicating that the cal file can be written. */
mHaveGoodMpuCal = true;
}
if(msg & INV_MSG_NEW_AB_EVENT) {
LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Accel Bias *****\n");
getAccelBias();
mAccelAccuracy = inv_get_accel_accuracy();
}
if(msg & INV_MSG_NEW_GB_EVENT) {
LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Gyro Bias *****\n");
getGyroBias();
setGyroBias();
}
if(msg & INV_MSG_NEW_FGB_EVENT) {
LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Factory Gyro Bias *****\n");
getFactoryGyroBias();
}
if(msg & INV_MSG_NEW_FAB_EVENT) {
LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Factory Accel Bias *****\n");
getFactoryAccelBias();
}
if(msg & INV_MSG_NEW_CB_EVENT) {
LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Compass Bias *****\n");
getCompassBias();
mCompassAccuracy = inv_get_mag_accuracy();
}
}
if (!mSkipReadEvents) {
for (int i = 0; i < NumSensors; i++) {
int update = 0;
// handle step detector when ped_q is enabled
if(mPedUpdate) {
if (i == StepDetector) {
update = readDmpPedometerEvents(data, count, ID_P, 1);
mPedUpdate = 0;
if(update == 1 && count > 0) {
if (mLastTimestamp[i] != mStepSensorTimestamp) {
count--;
numEventReceived++;
data->timestamp = mStepSensorTimestamp;
data++;
mLastTimestamp[i] = mStepSensorTimestamp;
} else {
ALOGE("Event from type=%d with duplicate timestamp %lld discarded",
mPendingEvents[i].type, mStepSensorTimestamp);
}
continue;
}
} else {
if (mPedUpdate == DATA_FORMAT_STEP) {
continue;
}
}
}
// load up virtual sensors
if (mEnabledCached & (1 << i)) {
update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
mPendingMask |= (1 << i);
if (update && (count > 0)) {
// Discard any events with duplicate timestamps
if (mLastTimestamp[i] != mPendingEvents[i].timestamp) {
mLastTimestamp[i] = mPendingEvents[i].timestamp;
*data++ = mPendingEvents[i];
count--;
numEventReceived++;
} else {
ALOGE("Event from type=%d with duplicate timestamp %lld (%+f, %+f, %+f) discarded",
mPendingEvents[i].type, mLastTimestamp[i], mPendingEvents[i].data[0], mPendingEvents[i].data[1], mPendingEvents[i].data[2]);
}
}
}
mCompassOverFlow = 0;
// handle partial packet read and end marker
// skip readEvents from hal_outputs
if (mFlushBatchSet && count>0 && !mFlushSensorEnabledVector.isEmpty()) {
while (mFlushBatchSet && count>0 && !mFlushSensorEnabledVector.isEmpty()) {
int sendEvent = metaHandler(&mPendingFlushEvents[0], META_DATA_FLUSH_COMPLETE);
if (sendEvent) {
LOGV_IF(ENG_VERBOSE, "Queueing flush complete for handle=%d",
mPendingFlushEvents[0].meta_data.sensor);
*data++ = mPendingFlushEvents[0];
count--;
numEventReceived++;
} else {
LOGV_IF(ENG_VERBOSE, "sendEvent false, NOT queueing flush complete for handle=%d",
mPendingFlushEvents[0].meta_data.sensor);
}
mFlushBatchSet--;
}
// Double check flush status
if (mFlushSensorEnabledVector.isEmpty()) {
mEmptyDataMarkerDetected = 0;
mDataMarkerDetected = 0;
mFlushBatchSet = 0;
LOGV_IF(ENG_VERBOSE, "Flush completed");
} else {
LOGV_IF(ENG_VERBOSE, "Flush is still active");
}
} else if (mFlushBatchSet && mFlushSensorEnabledVector.isEmpty()) {
mFlushBatchSet = 0;
}
}
}
return numEventReceived;
}
// collect data for MPL (but NOT sensor service currently), from driver layer
void MPLSensor::buildMpuEvent(void)
{
VHANDLER_LOG;
mSkipReadEvents = 0;
int64_t mGyroSensorTimestamp=0, mAccelSensorTimestamp=0, latestTimestamp=0;
int lp_quaternion_on = 0, sixAxis_quaternion_on = 0,
ped_quaternion_on = 0, ped_standalone_on = 0;
size_t nbyte;
unsigned short data_format = 0;
int i, nb, mask = 0,
sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
(((mLocalSensorMask & INV_THREE_AXIS_COMPASS)
&& mCompassSensor->isIntegrated())? 1 : 0) +
((mLocalSensorMask & INV_ONE_AXIS_PRESSURE)? 1 : 0);
char *rdata = mIIOBuffer;
ssize_t rsize = 0;
ssize_t readCounter = 0;
char *rdataP = NULL;
bool doneFlag = 0;
/* flush buffer when no sensors are enabled */
if (mEnabledCached == 0 && mBatchEnabled == 0 && mDmpPedometerEnabled == 0) {
rsize = read(iio_fd, rdata, MAX_SUSPEND_BATCH_PACKET_SIZE);
if(rsize > 0) {
LOGV_IF(ENG_VERBOSE, "HAL:input data flush rsize=%d", (int)rsize);
}
mLeftOverBufferSize = 0;
mDataMarkerDetected = 0;
mEmptyDataMarkerDetected = 0;
return;
}
lp_quaternion_on = isLowPowerQuatEnabled() && checkLPQuaternion();
sixAxis_quaternion_on = check6AxisQuatEnabled();
ped_quaternion_on = checkPedQuatEnabled();
ped_standalone_on = checkPedStandaloneEnabled();
nbyte = MAX_READ_SIZE - mLeftOverBufferSize;
/* check previous copied buffer */
/* append with just read data */
if (mLeftOverBufferSize > 0) {
LOGV_IF(0, "append old buffer size=%d", mLeftOverBufferSize);
memset(rdata, 0, sizeof(mIIOBuffer));
memcpy(rdata, mLeftOverBuffer, mLeftOverBufferSize);
LOGV_IF(0,
"HAL:input retrieve old buffer data=:%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, "
"%d, %d,%d, %d, %d, %d\n",
rdata[0], rdata[1], rdata[2], rdata[3],
rdata[4], rdata[5], rdata[6], rdata[7],
rdata[8], rdata[9], rdata[10], rdata[11],
rdata[12], rdata[13], rdata[14], rdata[15]);
}
rdataP = rdata + mLeftOverBufferSize;
/* read expected number of bytes */
rsize = read(iio_fd, rdataP, nbyte);
if(rsize < 0) {
/* IIO buffer might have old data.
Need to flush it if no sensor is on, to avoid infinite
read loop.*/
LOGE("HAL:input data file descriptor not available - (%s)",
strerror(errno));
if (sensors == 0) {
rsize = read(iio_fd, rdata, MAX_SUSPEND_BATCH_PACKET_SIZE);
if(rsize > 0) {
LOGV_IF(ENG_VERBOSE, "HAL:input data flush rsize=%d", (int)rsize);
#ifdef TESTING
LOGV_IF(INPUT_DATA,
"HAL:input rdata:r=%d, n=%d,"
"%d, %d, %d, %d, %d, %d, %d, %d",
(int)rsize, nbyte,
rdataP[0], rdataP[1], rdataP[2], rdataP[3],
rdataP[4], rdataP[5], rdataP[6], rdataP[7]);
#endif
mLeftOverBufferSize = 0;
}
}
return;
}
#ifdef TESTING
LOGV_IF(INPUT_DATA,
"HAL:input just read rdataP:r=%d, n=%d,"
"%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,"
"%d, %d, %d, %d,%d, %d, %d, %d\n",
(int)rsize, nbyte,
rdataP[0], rdataP[1], rdataP[2], rdataP[3],
rdataP[4], rdataP[5], rdataP[6], rdataP[7],
rdataP[8], rdataP[9], rdataP[10], rdataP[11],
rdataP[12], rdataP[13], rdataP[14], rdataP[15],
rdataP[16], rdataP[17], rdataP[18], rdataP[19],
rdataP[20], rdataP[21], rdataP[22], rdataP[23]);
LOGV_IF(INPUT_DATA,
"HAL:input rdata:r=%d, n=%d,"
"%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,"
"%d, %d, %d, %d,%d, %d, %d, %d\n",
(int)rsize, nbyte,
rdata[0], rdata[1], rdata[2], rdata[3],
rdata[4], rdata[5], rdata[6], rdata[7],
rdata[8], rdata[9], rdata[10], rdata[11],
rdata[12], rdata[13], rdata[14], rdata[15],
rdata[16], rdata[17], rdata[18], rdata[19],
rdata[20], rdata[21], rdata[22], rdata[23]);
#endif
/* reset data and count pointer */
rdataP = rdata;
readCounter = rsize + mLeftOverBufferSize;
LOGV_IF(0, "HAL:input readCounter set=%d", (int)readCounter);
if(readCounter < MAX_READ_SIZE) {
// Handle standalone MARKER packet
if (readCounter >= BYTES_PER_SENSOR) {
data_format = *((short *)(rdata));
if (data_format == DATA_FORMAT_MARKER) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "MARKER DETECTED:0x%x", data_format);
readCounter -= BYTES_PER_SENSOR;
rdata += BYTES_PER_SENSOR;
if (!mFlushSensorEnabledVector.isEmpty()) {
mFlushBatchSet++;
}
mDataMarkerDetected = 1;
}
else if (data_format == DATA_FORMAT_EMPTY_MARKER) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "EMPTY MARKER DETECTED:0x%x", data_format);
readCounter -= BYTES_PER_SENSOR;
rdata += BYTES_PER_SENSOR;
if (!mFlushSensorEnabledVector.isEmpty()) {
mFlushBatchSet++;
}
mEmptyDataMarkerDetected = 1;
mDataMarkerDetected = 1;
}
}
/* store packet then return */
mLeftOverBufferSize = readCounter;
memcpy(mLeftOverBuffer, rdata, mLeftOverBufferSize);
#ifdef TESTING
LOGV_IF(1, "HAL:input data has batched partial packet");
LOGV_IF(1, "HAL:input data batched mLeftOverBufferSize=%d", mLeftOverBufferSize);
LOGV_IF(1,
"HAL:input catch up batched retrieve buffer=:%d, %d, %d, %d, %d, %d, %d, %d,"
"%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3],
mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7],
mLeftOverBuffer[8], mLeftOverBuffer[9], mLeftOverBuffer[10], mLeftOverBuffer[11],
mLeftOverBuffer[12], mLeftOverBuffer[13], mLeftOverBuffer[14], mLeftOverBuffer[15]);
#endif
mSkipReadEvents = 1;
return;
}
LOGV_IF(INPUT_DATA && ENG_VERBOSE,
"HAL:input b=%d rdata= %d nbyte= %d rsize= %d readCounter= %d",
checkBatchEnabled(), *((short *) rdata), nbyte, (int)rsize, (int)readCounter);
LOGV_IF(INPUT_DATA && ENG_VERBOSE,
"HAL:input sensors= %d, lp_q_on= %d, 6axis_q_on= %d, "
"ped_q_on= %d, ped_standalone_on= %d",
sensors, lp_quaternion_on, sixAxis_quaternion_on, ped_quaternion_on,
ped_standalone_on);
mSkipExecuteOnData = 1;
while (readCounter > 0) {
// since copied buffer is already accounted for, reset left over size
mLeftOverBufferSize = 0;
// clear data format mask for parsing the next set of data
mask = 0;
data_format = *((short *)(rdata));
LOGV_IF(INPUT_DATA && ENG_VERBOSE,
"HAL:input data_format=%x", data_format);
if(checkValidHeader(data_format) == 0) {
LOGE("HAL:input invalid data_format 0x%02X", data_format);
return;
}
if (data_format & DATA_FORMAT_STEP) {
if (data_format == DATA_FORMAT_STEP) {
rdata += BYTES_PER_SENSOR;
latestTimestamp = *((long long*) (rdata));
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "STEP DETECTED:0x%x - ts: %lld", data_format, latestTimestamp);
// readCounter is decrement by 24 because DATA_FORMAT_STEP only applies in batch mode
readCounter -= BYTES_PER_SENSOR_PACKET;
} else {
LOGV_IF(0, "STEP DETECTED:0x%x", data_format);
}
mPedUpdate |= data_format;
// cancels step bit
data_format &= (~DATA_FORMAT_STEP);
}
if (data_format == DATA_FORMAT_MARKER) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "MARKER DETECTED:0x%x", data_format);
readCounter -= BYTES_PER_SENSOR;
if (!mFlushSensorEnabledVector.isEmpty()) {
mFlushBatchSet++;
}
mDataMarkerDetected = 1;
}
else if (data_format == DATA_FORMAT_EMPTY_MARKER) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "EMPTY MARKER DETECTED:0x%x", data_format);
readCounter -= BYTES_PER_SENSOR;
if (!mFlushSensorEnabledVector.isEmpty()) {
mFlushBatchSet++;
}
mEmptyDataMarkerDetected = 1;
mDataMarkerDetected = 1;
}
else if (data_format == DATA_FORMAT_QUAT) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "QUAT DETECTED:0x%x", data_format);
if (readCounter >= BYTES_QUAT_DATA) {
mCachedQuaternionData[0] = *((int *) (rdata + 4));
mCachedQuaternionData[1] = *((int *) (rdata + 8));
mCachedQuaternionData[2] = *((int *) (rdata + 12));
rdata += QUAT_ONLY_LAST_PACKET_OFFSET;
mQuatSensorTimestamp = *((long long*) (rdata));
mask |= DATA_FORMAT_QUAT;
readCounter -= BYTES_QUAT_DATA;
}
else {
doneFlag = 1;
}
}
else if (data_format == DATA_FORMAT_6_AXIS) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "6AXIS DETECTED:0x%x", data_format);
if (readCounter >= BYTES_QUAT_DATA) {
mCached6AxisQuaternionData[0] = *((int *) (rdata + 4));
mCached6AxisQuaternionData[1] = *((int *) (rdata + 8));
mCached6AxisQuaternionData[2] = *((int *) (rdata + 12));
rdata += QUAT_ONLY_LAST_PACKET_OFFSET;
mQuatSensorTimestamp = *((long long*) (rdata));
mask |= DATA_FORMAT_6_AXIS;
readCounter -= BYTES_QUAT_DATA;
}
else {
doneFlag = 1;
}
}
else if (data_format == DATA_FORMAT_PED_QUAT) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "PED QUAT DETECTED:0x%x", data_format);
if (readCounter >= BYTES_PER_SENSOR_PACKET) {
mCachedPedQuaternionData[0] = *((short *) (rdata + 2));
mCachedPedQuaternionData[1] = *((short *) (rdata + 4));
mCachedPedQuaternionData[2] = *((short *) (rdata + 6));
rdata += BYTES_PER_SENSOR;
mQuatSensorTimestamp = *((long long*) (rdata));
mask |= DATA_FORMAT_PED_QUAT;
readCounter -= BYTES_PER_SENSOR_PACKET;
}
else {
doneFlag = 1;
}
}
else if (data_format == DATA_FORMAT_PED_STANDALONE) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "STANDALONE STEP DETECTED:0x%x", data_format);
if (readCounter >= BYTES_PER_SENSOR_PACKET) {
rdata += BYTES_PER_SENSOR;
mStepSensorTimestamp = *((long long*) (rdata));
mask |= DATA_FORMAT_PED_STANDALONE;
readCounter -= BYTES_PER_SENSOR_PACKET;
mPedUpdate |= data_format;
}
else {
doneFlag = 1;
}
}
else if (data_format == DATA_FORMAT_GYRO) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "GYRO DETECTED:0x%x", data_format);
if (readCounter >= BYTES_PER_SENSOR_PACKET) {
mCachedGyroData[0] = *((short *) (rdata + 2));
mCachedGyroData[1] = *((short *) (rdata + 4));
mCachedGyroData[2] = *((short *) (rdata + 6));
rdata += BYTES_PER_SENSOR;
mGyroSensorTimestamp = *((long long*) (rdata));
mask |= DATA_FORMAT_GYRO;
readCounter -= BYTES_PER_SENSOR_PACKET;
} else {
doneFlag = 1;
}
}
else if (data_format == DATA_FORMAT_ACCEL) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "ACCEL DETECTED:0x%x", data_format);
if (readCounter >= BYTES_PER_SENSOR_PACKET) {
mCachedAccelData[0] = *((short *) (rdata + 2));
mCachedAccelData[1] = *((short *) (rdata + 4));
mCachedAccelData[2] = *((short *) (rdata + 6));
rdata += BYTES_PER_SENSOR;
mAccelSensorTimestamp = *((long long*) (rdata));
mask |= DATA_FORMAT_ACCEL;
readCounter -= BYTES_PER_SENSOR_PACKET;
}
else {
doneFlag = 1;
}
}
else if (data_format == DATA_FORMAT_COMPASS) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "COMPASS DETECTED:0x%x", data_format);
if (readCounter >= BYTES_PER_SENSOR_PACKET) {
if (mCompassSensor->isIntegrated()) {
mCachedCompassData[0] = *((short *) (rdata + 2));
mCachedCompassData[1] = *((short *) (rdata + 4));
mCachedCompassData[2] = *((short *) (rdata + 6));
rdata += BYTES_PER_SENSOR;
mCompassTimestamp = *((long long*) (rdata));
mask |= DATA_FORMAT_COMPASS;
readCounter -= BYTES_PER_SENSOR_PACKET;
}
}
else {
doneFlag = 1;
}
}
else if (data_format == DATA_FORMAT_COMPASS_OF) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "COMPASS OF DETECTED:0x%x", data_format);
mask |= DATA_FORMAT_COMPASS_OF;
mCompassOverFlow = 1;
#ifdef INV_PLAYBACK_DBG
if (readCounter >= BYTES_PER_SENSOR_PACKET) {
if (mCompassSensor->isIntegrated()) {
mCachedCompassData[0] = *((short *) (rdata + 2));
mCachedCompassData[1] = *((short *) (rdata + 4));
mCachedCompassData[2] = *((short *) (rdata + 6));
rdata += BYTES_PER_SENSOR;
mCompassTimestamp = *((long long*) (rdata));
readCounter -= BYTES_PER_SENSOR_PACKET;
}
}
#else
readCounter -= BYTES_PER_SENSOR;
#endif
}
#ifdef ENABLE_PRESSURE
else if (data_format == DATA_FORMAT_PRESSURE) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "PRESSURE DETECTED:0x%x", data_format);
if (readCounter >= BYTES_QUAT_DATA) {
if (mPressureSensor->isIntegrated()) {
mCachedPressureData =
((*((short *)(rdata + 4))) << 16) +
(*((unsigned short *) (rdata + 6)));
rdata += BYTES_PER_SENSOR;
mPressureTimestamp = *((long long*) (rdata));
if (mCachedPressureData != 0) {
mask |= DATA_FORMAT_PRESSURE;
}
readCounter -= BYTES_PER_SENSOR_PACKET;
}
} else{
doneFlag = 1;
}
}
#endif
if(doneFlag == 0) {
rdata += BYTES_PER_SENSOR;
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "HAL: input data doneFlag is zero, readCounter=%d", (int)readCounter);
}
else {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "HAL: input data doneFlag is set, readCounter=%d", (int)readCounter);
}
/* read ahead and store left over data if any */
if (readCounter != 0) {
int currentBufferCounter = 0;
LOGV_IF(0, "Not enough data readCounter=%d, expected nbyte=%d, rsize=%d", (int)readCounter, nbyte, (int)rsize);
memset(mLeftOverBuffer, 0, sizeof(mLeftOverBuffer));
/* check for end markers, don't save */
data_format = *((short *) (rdata));
if ((data_format == DATA_FORMAT_MARKER) || (data_format == DATA_FORMAT_EMPTY_MARKER)) {
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "s MARKER DETECTED:0x%x", data_format);
rdata += BYTES_PER_SENSOR;
readCounter -= BYTES_PER_SENSOR;
if (!mFlushSensorEnabledVector.isEmpty()) {
mFlushBatchSet++;
}
mDataMarkerDetected = 1;
if (readCounter == 0) {
mLeftOverBufferSize = 0;
if(doneFlag != 0) {
return;
}
}
}
memcpy(mLeftOverBuffer, rdata, readCounter);
LOGV_IF(0,
"HAL:input store rdata=:%d, %d, %d, %d,%d, %d, %d, %d,%d, "
"%d, %d, %d,%d, %d, %d, %d\n",
mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3],
mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7],
mLeftOverBuffer[8], mLeftOverBuffer[9], mLeftOverBuffer[10], mLeftOverBuffer[11],
mLeftOverBuffer[12],mLeftOverBuffer[13],mLeftOverBuffer[14], mLeftOverBuffer[15]);
mLeftOverBufferSize = readCounter;
readCounter = 0;
LOGV_IF(0, "Stored number of bytes:%d", mLeftOverBufferSize);
} else {
/* reset count since this is the last packet for the data set */
readCounter = 0;
mLeftOverBufferSize = 0;
}
/* handle data read */
if (mask == DATA_FORMAT_GYRO) {
/* batch mode does not batch temperature */
/* disable temperature read */
if (!(mFeatureActiveMask & INV_DMP_BATCH_MODE)) {
// send down temperature every 0.5 seconds
// with timestamp measured in "driver" layer
if(mGyroSensorTimestamp - mTempCurrentTime >= 500000000LL) {
mTempCurrentTime = mGyroSensorTimestamp;
long long temperature[2];
if(inv_read_temperature(temperature) == 0) {
LOGV_IF(INPUT_DATA,
"HAL:input inv_read_temperature = %lld, timestamp= %lld",
temperature[0], temperature[1]);
inv_build_temp(temperature[0], temperature[1]);
mSkipExecuteOnData = 0;
}
#ifdef TESTING
long bias[3], temp, temp_slope[3];
inv_get_mpl_gyro_bias(bias, &temp);
inv_get_gyro_ts(temp_slope);
LOGI("T: %.3f "
"GB: %+13f %+13f %+13f "
"TS: %+13f %+13f %+13f "
"\n",
(float)temperature[0] / 65536.f,
(float)bias[0] / 65536.f / 16.384f,
(float)bias[1] / 65536.f / 16.384f,
(float)bias[2] / 65536.f / 16.384f,
temp_slope[0] / 65536.f,
temp_slope[1] / 65536.f,
temp_slope[2] / 65536.f);
#endif
}
}
mPendingMask |= 1 << Gyro;
mPendingMask |= 1 << RawGyro;
inv_build_gyro(mCachedGyroData, mGyroSensorTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:input inv_build_gyro: %+8d %+8d %+8d - %lld",
mCachedGyroData[0], mCachedGyroData[1],
mCachedGyroData[2], mGyroSensorTimestamp);
mSkipExecuteOnData = 0;
latestTimestamp = mGyroSensorTimestamp;
}
if (mask == DATA_FORMAT_ACCEL) {
mPendingMask |= 1 << Accelerometer;
inv_build_accel(mCachedAccelData, 0, mAccelSensorTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:input inv_build_accel: %+8ld %+8ld %+8ld - %lld",
mCachedAccelData[0], mCachedAccelData[1],
mCachedAccelData[2], mAccelSensorTimestamp);
mSkipExecuteOnData = 0;
/* remember inital 6 axis quaternion */
inv_time_t tempTimestamp;
inv_get_6axis_quaternion(mInitial6QuatValue, &tempTimestamp);
if (mInitial6QuatValue[0] != 0 && mInitial6QuatValue[1] != 0 &&
mInitial6QuatValue[2] != 0 && mInitial6QuatValue[3] != 0) {
mInitial6QuatValueAvailable = 1;
LOGV_IF(INPUT_DATA && ENG_VERBOSE,
"HAL:input build 6q init: %+8ld %+8ld %+8ld %+8ld",
mInitial6QuatValue[0], mInitial6QuatValue[1],
mInitial6QuatValue[2], mInitial6QuatValue[3]);
}
latestTimestamp = mAccelSensorTimestamp;
}
if (mask == DATA_FORMAT_COMPASS_OF) {
/* compass overflow detected */
/* reset compass algorithm */
int status = 0;
inv_build_compass(mCachedCompassData, status,
mCompassTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:input inv_build_compass_of: %+8ld %+8ld %+8ld - %lld",
mCachedCompassData[0], mCachedCompassData[1],
mCachedCompassData[2], mCompassTimestamp);
mSkipExecuteOnData = 0;
resetCompass();
}
if ((mask == DATA_FORMAT_COMPASS) && mCompassSensor->isIntegrated()) {
int status = 0;
if (mCompassSensor->providesCalibration()) {
status = mCompassSensor->getAccuracy();
status |= INV_CALIBRATED;
}
inv_build_compass(mCachedCompassData, status,
mCompassTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
mCachedCompassData[0], mCachedCompassData[1],
mCachedCompassData[2], mCompassTimestamp);
mSkipExecuteOnData = 0;
latestTimestamp = mCompassTimestamp;
}
if (mask == DATA_FORMAT_QUAT) {
/* if bias was applied to DMP bias,
set status bits to disable gyro bias cal */
int status = 0;
if (mGyroBiasApplied == true) {
LOGV_IF(INPUT_DATA && ENG_VERBOSE, "HAL:input dmp bias is used");
status |= INV_BIAS_APPLIED;
}
status |= INV_CALIBRATED | INV_QUAT_3AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */
inv_build_quat(mCachedQuaternionData,
status,
mQuatSensorTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:input inv_build_quat-3x: %+8ld %+8ld %+8ld - %lld",
mCachedQuaternionData[0], mCachedQuaternionData[1],
mCachedQuaternionData[2],
mQuatSensorTimestamp);
mSkipExecuteOnData = 0;
latestTimestamp = mQuatSensorTimestamp;
}
if (mask == DATA_FORMAT_6_AXIS) {
/* if bias was applied to DMP bias,
set status bits to disable gyro bias cal */
int status = 0;
if (mGyroBiasApplied == true) {
LOGV_IF(INPUT_DATA && ENG_VERBOSE, "HAL:input dmp bias is used");
status |= INV_QUAT_6AXIS;
}
status |= INV_CALIBRATED | INV_QUAT_6AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */
inv_build_quat(mCached6AxisQuaternionData,
status,
mQuatSensorTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:input inv_build_quat-6x: %+8ld %+8ld %+8ld - %lld",
mCached6AxisQuaternionData[0], mCached6AxisQuaternionData[1],
mCached6AxisQuaternionData[2], mQuatSensorTimestamp);
mSkipExecuteOnData = 0;
latestTimestamp = mQuatSensorTimestamp;
}
if (mask == DATA_FORMAT_PED_QUAT) {
/* if bias was applied to DMP bias,
set status bits to disable gyro bias cal */
int status = 0;
if (mGyroBiasApplied == true) {
LOGV_IF(INPUT_DATA && ENG_VERBOSE,
"HAL:input dmp bias is used");
status |= INV_QUAT_6AXIS;
}
status |= INV_CALIBRATED | INV_QUAT_6AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */
mCachedPedQuaternionData[0] = mCachedPedQuaternionData[0] << 16;
mCachedPedQuaternionData[1] = mCachedPedQuaternionData[1] << 16;
mCachedPedQuaternionData[2] = mCachedPedQuaternionData[2] << 16;
inv_build_quat(mCachedPedQuaternionData,
status,
mQuatSensorTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:HAL:input inv_build_quat-ped_6x: %+8ld %+8ld %+8ld - %lld",
mCachedPedQuaternionData[0], mCachedPedQuaternionData[1],
mCachedPedQuaternionData[2], mQuatSensorTimestamp);
mSkipExecuteOnData = 0;
latestTimestamp = mQuatSensorTimestamp;
}
#ifdef ENABLE_PRESSURE
if ((mask ==DATA_FORMAT_PRESSURE) && mPressureSensor->isIntegrated()) {
int status = 0;
latestTimestamp = mPressureTimestamp;
mPressureUpdate = 1;
inv_build_pressure(mCachedPressureData,
status,
mPressureTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:input inv_build_pressure: %+8ld - %lld",
mCachedPressureData, mPressureTimestamp);
mSkipExecuteOnData = 0;
}
#endif
/* take the latest timestamp */
if (mPedUpdate & DATA_FORMAT_STEP) {
/* work around driver output duplicate step detector bit */
if (latestTimestamp > mStepSensorTimestamp) {
mStepSensorTimestamp = latestTimestamp;
LOGV_IF(INPUT_DATA,
"HAL:input build step: 1 - %lld", mStepSensorTimestamp);
} else {
LOGV_IF(ENG_VERBOSE, "Step data OUT OF ORDER, "
"mPedUpdate = 0x%x last = %lld, ts = %lld",
mPedUpdate, mStepSensorTimestamp, latestTimestamp);
mPedUpdate = 0;
}
}
} //while end
}
int MPLSensor::checkValidHeader(unsigned short data_format)
{
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "check data_format=%x", data_format);
if(data_format == DATA_FORMAT_STEP)
return 1;
if(data_format & DATA_FORMAT_STEP) {
data_format &= (~DATA_FORMAT_STEP);
}
if ((data_format == DATA_FORMAT_PED_STANDALONE) ||
(data_format == DATA_FORMAT_PED_QUAT) ||
(data_format == DATA_FORMAT_6_AXIS) ||
(data_format == DATA_FORMAT_QUAT) ||
(data_format == DATA_FORMAT_COMPASS) ||
(data_format == DATA_FORMAT_GYRO) ||
(data_format == DATA_FORMAT_ACCEL) ||
(data_format == DATA_FORMAT_PRESSURE) ||
(data_format == DATA_FORMAT_EMPTY_MARKER) ||
(data_format == DATA_FORMAT_MARKER) ||
(data_format == DATA_FORMAT_COMPASS_OF))
return 1;
else {
LOGV_IF(ENG_VERBOSE, "bad data_format = %x", data_format);
return 0;
}
}
/* use for both MPUxxxx and third party compass */
void MPLSensor::buildCompassEvent(void)
{
VHANDLER_LOG;
int done = 0;
// pthread_mutex_lock(&mMplMutex);
// pthread_mutex_lock(&mHALMutex);
done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
if(mCompassSensor->isYasCompass()) {
if (mCompassSensor->checkCoilsReset() == 1) {
//Reset relevant compass settings
resetCompass();
}
}
if (done > 0) {
int status = 0;
if (mCompassSensor->providesCalibration()) {
status = mCompassSensor->getAccuracy();
status |= INV_CALIBRATED;
}
inv_build_compass(mCachedCompassData, status,
mCompassTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
mCachedCompassData[0], mCachedCompassData[1],
mCachedCompassData[2], mCompassTimestamp);
mSkipReadEvents = 0;
mSkipExecuteOnData = 0;
}
// pthread_mutex_unlock(&mMplMutex);
// pthread_mutex_unlock(&mHALMutex);
}
int MPLSensor::resetCompass(void)
{
VFUNC_LOG;
//Reset compass cal if enabled
if (mMplFeatureActiveMask & INV_COMPASS_CAL) {
LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
inv_init_vector_compass_cal();
inv_init_magnetic_disturbance();
inv_vector_compass_cal_sensitivity(3);
}
//Reset compass fit if enabled
if (mMplFeatureActiveMask & INV_COMPASS_FIT) {
LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
inv_init_compass_fit();
}
return 0;
}
int MPLSensor::getFd(void) const
{
VFUNC_LOG;
LOGV_IF(EXTRA_VERBOSE, "getFd returning %d", iio_fd);
return iio_fd;
}
int MPLSensor::getAccelFd(void) const
{
VFUNC_LOG;
LOGV_IF(EXTRA_VERBOSE, "getAccelFd returning %d", accel_fd);
return accel_fd;
}
int MPLSensor::getCompassFd(void) const
{
VFUNC_LOG;
int fd = mCompassSensor->getFd();
LOGV_IF(EXTRA_VERBOSE, "getCompassFd returning %d", fd);
return fd;
}
int MPLSensor::turnOffAccelFifo(void)
{
VFUNC_LOG;
int i, res = 0, tempFd;
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
0, mpu.accel_fifo_enable, getTimestamp());
res += write_sysfs_int(mpu.accel_fifo_enable, 0);
return res;
}
int MPLSensor::turnOffGyroFifo(void)
{
VFUNC_LOG;
int i, res = 0, tempFd;
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
0, mpu.gyro_fifo_enable, getTimestamp());
res += write_sysfs_int(mpu.gyro_fifo_enable, 0);
return res;
}
int MPLSensor::enableDmpOrientation(int en)
{
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 orientation
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.display_orientation_on, getTimestamp());
if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
LOGE("HAL:ERR can't enable Android orientation");
res = -1; // indicate an err
return res;
}
// enable accel engine
res = enableAccel(1);
if (res < 0)
return res;
// disable accel FIFO
if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
res = turnOffAccelFifo();
if (res < 0)
return res;
}
if (!mEnabled){
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) {
res = -1;
LOGE("HAL:ERR can't enable DMP event interrupt");
}
}
mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
} else {
mFeatureActiveMask &= ~INV_DMP_DISPL_ORIENTATION;
if (mFeatureActiveMask == 0) {
// disable accel engine
if (!(mLocalSensorMask & mMasterSensorMask
& INV_THREE_AXIS_ACCEL)) {
res = enableAccel(0);
if (res < 0)
return res;
}
}
if (mEnabled){
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");
}
}
LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
}
if ((res = computeAndSetDmpState()) < 0)
return res;
if (en || mEnabled || mFeatureActiveMask) {
res = masterEnable(1);
}
return res;
}
int MPLSensor::openDmpOrientFd(void)
{
VFUNC_LOG;
if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
LOGV_IF(PROCESS_VERBOSE,
"HAL:DMP display orientation disabled or file desc opened");
return 0;
}
dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
if (dmp_orient_fd < 0) {
LOGE("HAL:ERR couldn't open dmpOrient node");
return -1;
} else {
LOGV_IF(PROCESS_VERBOSE,
"HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
return 0;
}
}
int MPLSensor::closeDmpOrientFd(void)
{
VFUNC_LOG;
if (dmp_orient_fd >= 0)
close(dmp_orient_fd);
return 0;
}
int MPLSensor::dmpOrientHandler(int orient)
{
VFUNC_LOG;
LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
return 0;
}
int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count)
{
VFUNC_LOG;
char dummy[4];
int screen_orientation = 0;
FILE *fp;
fp = fopen(mpu.event_display_orientation, "r");
if (fp == NULL) {
LOGE("HAL:cannot open event_display_orientation");
return 0;
} else {
if (fscanf(fp, "%d\n", &screen_orientation) < 0 || fclose(fp) < 0)
{
LOGE("HAL:cannot write event_display_orientation");
}
}
int numEventReceived = 0;
if (mDmpOrientationEnabled && count > 0) {
sensors_event_t temp;
temp.acceleration.x = 0;
temp.acceleration.y = 0;
temp.acceleration.z = 0;
temp.version = sizeof(sensors_event_t);
temp.sensor = ID_SO;
temp.acceleration.status
= SENSOR_STATUS_UNRELIABLE;
#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
temp.screen_orientation = screen_orientation;
#endif
temp.timestamp = android::elapsedRealtimeNano();
*data++ = temp;
count--;
numEventReceived++;
}
// read dummy data per driver's request
dmpOrientHandler(screen_orientation);
read(dmp_orient_fd, dummy, 4);
return numEventReceived;
}
int MPLSensor::getDmpOrientFd(void)
{
VFUNC_LOG;
LOGV_IF(EXTRA_VERBOSE, "getDmpOrientFd returning %d", dmp_orient_fd);
return dmp_orient_fd;
}
int MPLSensor::checkDMPOrientation(void)
{
VFUNC_LOG;
return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
}
int MPLSensor::getDmpRate(int64_t *wanted)
{
VFUNC_LOG;
// set DMP output rate to FIFO
if(mDmpOn == 1) {
setQuaternionRate(*wanted);
if (mFeatureActiveMask & INV_DMP_BATCH_MODE) {
set6AxisQuaternionRate(*wanted);
setPedQuaternionRate(*wanted);
}
// DMP running rate must be @ 200Hz
*wanted= RATE_200HZ;
LOGV_IF(PROCESS_VERBOSE,
"HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
}
return 0;
}
int MPLSensor::getPollTime(void)
{
VFUNC_LOG;
return mPollTime;
}
int MPLSensor::getStepCountPollTime(void)
{
VFUNC_LOG;
if (mDmpStepCountEnabled) {
// convert poll time from nS to mS
return (mStepCountPollTime / 1000000LL);
}
return 1000;
}
bool MPLSensor::hasStepCountPendingEvents(void)
{
VFUNC_LOG;
if (mDmpStepCountEnabled) {
int64_t t_now_ns;
int64_t interval = 0;
t_now_ns = android::elapsedRealtimeNano();
interval = t_now_ns - mt_pre_ns;
if (interval < mStepCountPollTime) {
LOGV_IF(0,
"Step Count interval elapsed: %lld, triggered: %lld",
interval, mStepCountPollTime);
return false;
} else {
mt_pre_ns = android::elapsedRealtimeNano();
LOGV_IF(0, "Step Count previous time: %lld ms",
mt_pre_ns / 1000000);
return true;
}
}
return false;
}
bool MPLSensor::hasPendingEvents(void) const
{
VFUNC_LOG;
// if we are using the polling workaround, force the main
// loop to check for data every time
return (mPollTime != -1);
}
int MPLSensor::inv_read_temperature(long long *data)
{
VHANDLER_LOG;
int count = 0;
char raw_buf[40];
long raw = 0;
long long timestamp = 0;
memset(raw_buf, 0, sizeof(raw_buf));
count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
sizeof(raw_buf));
if(count < 1) {
LOGE("HAL:error reading gyro temperature");
return -1;
}
count = sscanf(raw_buf, "%ld%lld", &raw, ×tamp);
if(count < 0) {
return -1;
}
LOGV_IF(ENG_VERBOSE && INPUT_DATA,
"HAL:temperature raw = %ld, timestamp = %lld, count = %d",
raw, timestamp, count);
data[0] = raw;
data[1] = timestamp;
return 0;
}
int MPLSensor::inv_read_dmp_state(int fd)
{
VFUNC_LOG;
if(fd < 0)
return -1;
int count = 0;
char raw_buf[10];
short raw = 0;
memset(raw_buf, 0, sizeof(raw_buf));
count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
if(count < 1) {
LOGE("HAL:error reading dmp state");
close(fd);
return -1;
}
count = sscanf(raw_buf, "%hd", &raw);
if(count < 0) {
LOGE("HAL:dmp state data is invalid");
close(fd);
return -1;
}
LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
close(fd);
return (int)raw;
}
int MPLSensor::inv_read_sensor_bias(int fd, long *data)
{
VFUNC_LOG;
if(fd == -1) {
return -1;
}
char buf[50];
char x[15], y[15], z[15];
memset(buf, 0, sizeof(buf));
int count = read_attribute_sensor(fd, buf, sizeof(buf));
if(count < 1) {
LOGE("HAL:Error reading gyro bias");
return -1;
}
count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
if(count) {
/* scale appropriately for MPL */
LOGV_IF(ENG_VERBOSE,
"HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
atol(x), atol(y), atol(z));
data[0] = (long)(atol(x) / 10000 * (1L << 16));
data[1] = (long)(atol(y) / 10000 * (1L << 16));
data[2] = (long)(atol(z) / 10000 * (1L << 16));
LOGV_IF(ENG_VERBOSE,
"HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
data[0], data[1], data[2]);
}
return 0;
}
/** fill in the sensor list based on which sensors are configured.
* return the number of configured sensors.
* parameter list must point to a memory region of at least 7*sizeof(sensor_t)
* parameter len gives the length of the buffer pointed to by list
*/
int MPLSensor::populateSensorList(struct sensor_t *list, int len)
{
VFUNC_LOG;
int numsensors;
if(len <
(int)((sizeof(sBaseSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
LOGE("HAL:sensor list too small, not populating.");
return -(sizeof(sBaseSensorList) / sizeof(sensor_t));
}
/* fill in the base values */
memcpy(list, sBaseSensorList,
sizeof (struct sensor_t) * (sizeof(sBaseSensorList) / sizeof(sensor_t)));
/* first add gyro, accel and compass to the list */
/* fill in gyro/accel values */
if(chip_ID == NULL) {
LOGE("HAL:Can not get gyro/accel id");
}
fillGyro(chip_ID, list);
fillAccel(chip_ID, list);
// TODO: need fixes for unified HAL and 3rd-party solution
mCompassSensor->fillList(&list[MagneticField]);
mCompassSensor->fillList(&list[RawMagneticField]);
#ifdef ENABLE_PRESSURE
if (mPressureSensor != NULL) {
mPressureSensor->fillList(&list[Pressure]);
}
#endif
if(1) {
numsensors = (sizeof(sBaseSensorList) / sizeof(sensor_t));
/* all sensors will be added to the list
fill in orientation values */
fillOrientation(list);
/* fill in rotation vector values */
fillRV(list);
/* fill in game rotation vector values */
fillGRV(list);
/* fill in gravity values */
fillGravity(list);
/* fill in Linear accel values */
fillLinearAccel(list);
/* fill in Significant motion values */
fillSignificantMotion(list);
#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
/* fill in screen orientation values */
fillScreenOrientation(list);
#endif
} else {
/* no 9-axis sensors, zero fill that part of the list */
numsensors = 3;
memset(list + 3, 0, 4 * sizeof(struct sensor_t));
}
return numsensors;
}
void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
{
VFUNC_LOG;
if (accel) {
if(accel != NULL && strcmp(accel, "BMA250") == 0) {
list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
list[Accelerometer].power = ACCEL_BMA250_POWER;
list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
return;
} else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
list[Accelerometer].power = ACCEL_MPU6050_POWER;
list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
return;
} else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
list[Accelerometer].power = ACCEL_MPU6500_POWER;
list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
return;
} else if (accel != NULL && strcmp(accel, "MPU6515") == 0) {
list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
list[Accelerometer].power = ACCEL_MPU6500_POWER;
list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
return;
} else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
list[Accelerometer].power = ACCEL_MPU9150_POWER;
list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
return;
} else if (accel != NULL && strcmp(accel, "MPU9250") == 0) {
list[Accelerometer].maxRange = ACCEL_MPU9250_RANGE;
list[Accelerometer].resolution = ACCEL_MPU9250_RESOLUTION;
list[Accelerometer].power = ACCEL_MPU9250_POWER;
list[Accelerometer].minDelay = ACCEL_MPU9250_MINDELAY;
return;
} else if (accel != NULL && strcmp(accel, "MPU9255") == 0) {
list[Accelerometer].maxRange = ACCEL_MPU9255_RANGE;
list[Accelerometer].resolution = ACCEL_MPU9255_RESOLUTION;
list[Accelerometer].power = ACCEL_MPU9255_POWER;
list[Accelerometer].minDelay = ACCEL_MPU9255_MINDELAY;
return;
} else if (accel != NULL && strcmp(accel, "MPU9350") == 0) {
list[Accelerometer].maxRange = ACCEL_MPU9350_RANGE;
list[Accelerometer].resolution = ACCEL_MPU9350_RESOLUTION;
list[Accelerometer].power = ACCEL_MPU9350_POWER;
list[Accelerometer].minDelay = ACCEL_MPU9350_MINDELAY;
return;
} else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
list[Accelerometer].power = ACCEL_BMA250_POWER;
list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
return;
}
}
LOGE("HAL:unknown accel id %s -- "
"params default to mpu6515 and might be wrong.",
accel);
list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
list[Accelerometer].power = ACCEL_MPU6500_POWER;
list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
}
void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
{
VFUNC_LOG;
if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
list[Gyro].maxRange = GYRO_MPU3050_RANGE;
list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
list[Gyro].power = GYRO_MPU3050_POWER;
list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
} else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
list[Gyro].maxRange = GYRO_MPU6050_RANGE;
list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
list[Gyro].power = GYRO_MPU6050_POWER;
list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
} else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
list[Gyro].maxRange = GYRO_MPU6500_RANGE;
list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
list[Gyro].power = GYRO_MPU6500_POWER;
list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
} else if( gyro != NULL && strcmp(gyro, "MPU6515") == 0) {
list[Gyro].maxRange = GYRO_MPU6500_RANGE;
list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
list[Gyro].power = GYRO_MPU6500_POWER;
list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
} else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
list[Gyro].maxRange = GYRO_MPU9150_RANGE;
list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
list[Gyro].power = GYRO_MPU9150_POWER;
list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
} else if( gyro != NULL && strcmp(gyro, "MPU9250") == 0) {
list[Gyro].maxRange = GYRO_MPU9250_RANGE;
list[Gyro].resolution = GYRO_MPU9250_RESOLUTION;
list[Gyro].power = GYRO_MPU9250_POWER;
list[Gyro].minDelay = GYRO_MPU9250_MINDELAY;
} else if( gyro != NULL && strcmp(gyro, "MPU9255") == 0) {
list[Gyro].maxRange = GYRO_MPU9255_RANGE;
list[Gyro].resolution = GYRO_MPU9255_RESOLUTION;
list[Gyro].power = GYRO_MPU9255_POWER;
list[Gyro].minDelay = GYRO_MPU9255_MINDELAY;
} else if( gyro != NULL && strcmp(gyro, "MPU9350") == 0) {
list[Gyro].maxRange = GYRO_MPU9350_RANGE;
list[Gyro].resolution = GYRO_MPU9350_RESOLUTION;
list[Gyro].power = GYRO_MPU9350_POWER;
list[Gyro].minDelay = GYRO_MPU9350_MINDELAY;
} else {
LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
LOGE("HAL:default to use mpu6515 params");
list[Gyro].maxRange = GYRO_MPU6500_RANGE;
list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
list[Gyro].power = GYRO_MPU6500_POWER;
list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
}
list[RawGyro].maxRange = list[Gyro].maxRange;
list[RawGyro].resolution = list[Gyro].resolution;
list[RawGyro].power = list[Gyro].power;
list[RawGyro].minDelay = list[Gyro].minDelay;
return;
}
/* fillRV depends on values of gyro, accel and compass in the list */
void MPLSensor::fillRV(struct sensor_t *list)
{
VFUNC_LOG;
/* compute power on the fly */
list[RotationVector].power = list[Gyro].power +
list[Accelerometer].power +
list[MagneticField].power;
list[RotationVector].resolution = .00001;
list[RotationVector].maxRange = 1.0;
list[RotationVector].minDelay = 5000;
return;
}
/* fillGMRV depends on values of accel and mag in the list */
void MPLSensor::fillGMRV(struct sensor_t *list)
{
VFUNC_LOG;
/* compute power on the fly */
list[GeomagneticRotationVector].power = list[Accelerometer].power +
list[MagneticField].power;
list[GeomagneticRotationVector].resolution = .00001;
list[GeomagneticRotationVector].maxRange = 1.0;
list[GeomagneticRotationVector].minDelay = 5000;
return;
}
/* fillGRV depends on values of gyro and accel in the list */
void MPLSensor::fillGRV(struct sensor_t *list)
{
VFUNC_LOG;
/* compute power on the fly */
list[GameRotationVector].power = list[Gyro].power +
list[Accelerometer].power;
list[GameRotationVector].resolution = .00001;
list[GameRotationVector].maxRange = 1.0;
list[GameRotationVector].minDelay = 5000;
return;
}
void MPLSensor::fillOrientation(struct sensor_t *list)
{
VFUNC_LOG;
list[Orientation].power = list[Gyro].power +
list[Accelerometer].power +
list[MagneticField].power;
list[Orientation].resolution = .00001;
list[Orientation].maxRange = 360.0;
list[Orientation].minDelay = 5000;
return;
}
void MPLSensor::fillGravity( struct sensor_t *list)
{
VFUNC_LOG;
list[Gravity].power = list[Gyro].power +
list[Accelerometer].power +
list[MagneticField].power;
list[Gravity].resolution = .00001;
list[Gravity].maxRange = 9.81;
list[Gravity].minDelay = 5000;
return;
}
void MPLSensor::fillLinearAccel(struct sensor_t *list)
{
VFUNC_LOG;
list[LinearAccel].power = list[Gyro].power +
list[Accelerometer].power +
list[MagneticField].power;
list[LinearAccel].resolution = list[Accelerometer].resolution;
list[LinearAccel].maxRange = list[Accelerometer].maxRange;
list[LinearAccel].minDelay = 5000;
return;
}
void MPLSensor::fillSignificantMotion(struct sensor_t *list)
{
VFUNC_LOG;
list[SignificantMotion].power = list[Accelerometer].power;
list[SignificantMotion].resolution = 1;
list[SignificantMotion].maxRange = 1;
list[SignificantMotion].minDelay = -1;
}
#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
void MPLSensor::fillScreenOrientation(struct sensor_t *list)
{
VFUNC_LOG;
list[NumSensors].power = list[Accelerometer].power;
list[NumSensors].resolution = 1;
list[NumSensors].maxRange = 3;
list[NumSensors].minDelay = 0;
}
#endif
int MPLSensor::inv_init_sysfs_attributes(void)
{
VFUNC_LOG;
char sysfs_path[MAX_SYSFS_NAME_LEN];
memset(sysfs_path, 0, sizeof(sysfs_path));
sysfs_names_ptr = (char*)calloc(MAX_SYSFS_ATTRB,
sizeof(char[MAX_SYSFS_NAME_LEN]));
if (sysfs_names_ptr == NULL) {
LOGE("HAL:couldn't alloc mem for sysfs paths");
return -1;
}
char *sptr = sysfs_names_ptr;
char **dptr = reinterpret_cast<char **>(&mpu);
for (size_t i = 0; i < MAX_SYSFS_ATTRB; i++) {
*dptr++ = sptr;
sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
}
// get proper (in absolute) IIO path & build MPU's sysfs paths
inv_get_sysfs_path(sysfs_path);
memcpy(mSysfsPath, sysfs_path, sizeof(sysfs_path));
sprintf(mpu.key, "%s%s", sysfs_path, "/key");
sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
sprintf(mpu.master_enable, "%s%s", sysfs_path, "/master_enable");
sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path,
"/scan_elements/in_timestamp_en");
sprintf(mpu.in_timestamp_index, "%s%s", sysfs_path,
"/scan_elements/in_timestamp_index");
sprintf(mpu.in_timestamp_type, "%s%s", sysfs_path,
"/scan_elements/in_timestamp_type");
sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
sprintf(mpu.dmp_on, "%s%s", sysfs_path, "/dmp_on");
sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
sprintf(mpu.dmp_event_int_on, "%s%s", sysfs_path, "/dmp_event_int_on");
sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable");
sprintf(mpu.gyro_fsr, "%s%s", sysfs_path, "/in_anglvel_scale");
sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable");
sprintf(mpu.gyro_rate, "%s%s", sysfs_path, "/gyro_rate");
sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accel_enable");
sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accel_matrix");
sprintf(mpu.accel_fifo_enable, "%s%s", sysfs_path, "/accel_fifo_enable");
sprintf(mpu.accel_rate, "%s%s", sysfs_path, "/accel_rate");
#ifndef THIRD_PARTY_ACCEL //MPU3050
sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
// DMP uses these values
sprintf(mpu.in_accel_x_dmp_bias, "%s%s", sysfs_path, "/in_accel_x_dmp_bias");
sprintf(mpu.in_accel_y_dmp_bias, "%s%s", sysfs_path, "/in_accel_y_dmp_bias");
sprintf(mpu.in_accel_z_dmp_bias, "%s%s", sysfs_path, "/in_accel_z_dmp_bias");
// MPU uses these values
sprintf(mpu.in_accel_x_offset, "%s%s", sysfs_path, "/in_accel_x_offset");
sprintf(mpu.in_accel_y_offset, "%s%s", sysfs_path, "/in_accel_y_offset");
sprintf(mpu.in_accel_z_offset, "%s%s", sysfs_path, "/in_accel_z_offset");
sprintf(mpu.in_accel_self_test_scale, "%s%s", sysfs_path, "/in_accel_self_test_scale");
#endif
// DMP uses these bias values
sprintf(mpu.in_gyro_x_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_x_dmp_bias");
sprintf(mpu.in_gyro_y_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_y_dmp_bias");
sprintf(mpu.in_gyro_z_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_z_dmp_bias");
// MPU uses these bias values
sprintf(mpu.in_gyro_x_offset, "%s%s", sysfs_path, "/in_anglvel_x_offset");
sprintf(mpu.in_gyro_y_offset, "%s%s", sysfs_path, "/in_anglvel_y_offset");
sprintf(mpu.in_gyro_z_offset, "%s%s", sysfs_path, "/in_anglvel_z_offset");
sprintf(mpu.in_gyro_self_test_scale, "%s%s", sysfs_path, "/in_anglvel_self_test_scale");
sprintf(mpu.three_axis_q_on, "%s%s", sysfs_path, "/three_axes_q_on"); //formerly quaternion_on
sprintf(mpu.three_axis_q_rate, "%s%s", sysfs_path, "/three_axes_q_rate");
sprintf(mpu.ped_q_on, "%s%s", sysfs_path, "/ped_q_on");
sprintf(mpu.ped_q_rate, "%s%s", sysfs_path, "/ped_q_rate");
sprintf(mpu.six_axis_q_on, "%s%s", sysfs_path, "/six_axes_q_on");
sprintf(mpu.six_axis_q_rate, "%s%s", sysfs_path, "/six_axes_q_rate");
sprintf(mpu.six_axis_q_value, "%s%s", sysfs_path, "/six_axes_q_value");
sprintf(mpu.step_detector_on, "%s%s", sysfs_path, "/step_detector_on");
sprintf(mpu.step_indicator_on, "%s%s", sysfs_path, "/step_indicator_on");
sprintf(mpu.display_orientation_on, "%s%s", sysfs_path,
"/display_orientation_on");
sprintf(mpu.event_display_orientation, "%s%s", sysfs_path,
"/event_display_orientation");
sprintf(mpu.event_smd, "%s%s", sysfs_path,
"/event_smd");
sprintf(mpu.smd_enable, "%s%s", sysfs_path,
"/smd_enable");
sprintf(mpu.smd_delay_threshold, "%s%s", sysfs_path,
"/smd_delay_threshold");
sprintf(mpu.smd_delay_threshold2, "%s%s", sysfs_path,
"/smd_delay_threshold2");
sprintf(mpu.smd_threshold, "%s%s", sysfs_path,
"/smd_threshold");
sprintf(mpu.batchmode_timeout, "%s%s", sysfs_path,
"/batchmode_timeout");
sprintf(mpu.batchmode_wake_fifo_full_on, "%s%s", sysfs_path,
"/batchmode_wake_fifo_full_on");
sprintf(mpu.flush_batch, "%s%s", sysfs_path,
"/flush_batch");
sprintf(mpu.pedometer_on, "%s%s", sysfs_path,
"/pedometer_on");
sprintf(mpu.pedometer_int_on, "%s%s", sysfs_path,
"/pedometer_int_on");
sprintf(mpu.event_pedometer, "%s%s", sysfs_path,
"/event_pedometer");
sprintf(mpu.pedometer_steps, "%s%s", sysfs_path,
"/pedometer_steps");
sprintf(mpu.pedometer_step_thresh, "%s%s", sysfs_path,
"/pedometer_step_thresh");
sprintf(mpu.pedometer_counter, "%s%s", sysfs_path,
"/pedometer_counter");
sprintf(mpu.motion_lpa_on, "%s%s", sysfs_path,
"/motion_lpa_on");
return 0;
}
//DMP support only for MPU6xxx/9xxx
bool MPLSensor::isMpuNonDmp(void)
{
VFUNC_LOG;
if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050"))
return true;
else
return false;
}
int MPLSensor::isLowPowerQuatEnabled(void)
{
VFUNC_LOG;
#ifdef ENABLE_LP_QUAT_FEAT
return !isMpuNonDmp();
#else
return 0;
#endif
}
int MPLSensor::isDmpDisplayOrientationOn(void)
{
VFUNC_LOG;
#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
if (isMpuNonDmp())
return 0;
return 1;
#else
return 0;
#endif
}
/* these functions can be consolidated
with inv_convert_to_body_with_scale */
void MPLSensor::getCompassBias()
{
VFUNC_LOG;
long bias[3];
long compassBias[3];
unsigned short orient;
signed char orientMtx[9];
mCompassSensor->getOrientationMatrix(orientMtx);
orient = inv_orientation_matrix_to_scalar(orientMtx);
/* Get Values from MPL */
inv_get_compass_bias(bias);
inv_convert_to_body(orient, bias, compassBias);
LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) %ld %ld %ld", bias[0], bias[1], bias[2]);
LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) (body) %ld %ld %ld", compassBias[0], compassBias[1], compassBias[2]);
long compassSensitivity = inv_get_compass_sensitivity();
if (compassSensitivity == 0) {
compassSensitivity = mCompassScale;
}
for(int i=0; i<3; i++) {
/* convert to uT */
float temp = (float) compassSensitivity / (1L << 30);
mCompassBias[i] =(float) (compassBias[i] * temp / 65536.f);
}
return;
}
void MPLSensor::getFactoryGyroBias()
{
VFUNC_LOG;
/* Get Values from MPL */
inv_get_gyro_bias(mFactoryGyroBias);
LOGV_IF(ENG_VERBOSE, "Factory Gyro Bias %ld %ld %ld", mFactoryGyroBias[0], mFactoryGyroBias[1], mFactoryGyroBias[2]);
mFactoryGyroBiasAvailable = true;
return;
}
/* set bias from factory cal file to MPU offset (in chip frame)
x = values store in cal file --> (v/1000 * 2^16 / (2000/250))
offset = x/2^16 * (Gyro scale / self test scale used) * (-1) / offset scale
i.e. self test default scale = 250
gyro scale default to = 2000
offset scale = 4 //as spec by hardware
offset = x/2^16 * (8) * (-1) / (4)
*/
void MPLSensor::setFactoryGyroBias()
{
VFUNC_LOG;
int scaleRatio = mGyroScale / mGyroSelfTestScale;
int offsetScale = 4;
LOGV_IF(ENG_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio);
LOGV_IF(ENG_VERBOSE, "HAL: offsetScale used =%d", offsetScale);
/* Write to Driver */
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
(((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale),
mpu.in_gyro_x_offset, getTimestamp());
if(write_attribute_sensor_continuous(gyro_x_offset_fd,
(((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
{
LOGE("HAL:Error writing to gyro_x_offset");
return;
}
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
(((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale),
mpu.in_gyro_y_offset, getTimestamp());
if(write_attribute_sensor_continuous(gyro_y_offset_fd,
(((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
{
LOGE("HAL:Error writing to gyro_y_offset");
return;
}
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
(((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale),
mpu.in_gyro_z_offset, getTimestamp());
if(write_attribute_sensor_continuous(gyro_z_offset_fd,
(((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
{
LOGE("HAL:Error writing to gyro_z_offset");
return;
}
mFactoryGyroBiasAvailable = false;
LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Gyro Calibrated Bias Applied");
return;
}
/* these functions can be consolidated
with inv_convert_to_body_with_scale */
void MPLSensor::getGyroBias()
{
VFUNC_LOG;
long *temp = NULL;
long chipBias[3];
long bias[3];
unsigned short orient;
/* Get Values from MPL */
inv_get_mpl_gyro_bias(mGyroChipBias, temp);
orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
inv_convert_to_body(orient, mGyroChipBias, bias);
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "Mpl Gyro Bias (HW unit) %ld %ld %ld", mGyroChipBias[0], mGyroChipBias[1], mGyroChipBias[2]);
LOGV_IF(ENG_VERBOSE && INPUT_DATA, "Mpl Gyro Bias (HW unit) (body) %ld %ld %ld", bias[0], bias[1], bias[2]);
long gyroSensitivity = inv_get_gyro_sensitivity();
if(gyroSensitivity == 0) {
gyroSensitivity = mGyroScale;
}
/* scale and convert to rad */
for(int i=0; i<3; i++) {
float temp = (float) gyroSensitivity / (1L << 30);
mGyroBias[i] = (float) (bias[i] * temp / (1<<16) / 180 * M_PI);
if (mGyroBias[i] != 0)
mGyroBiasAvailable = true;
}
return;
}
void MPLSensor::setGyroZeroBias()
{
VFUNC_LOG;
/* Write to Driver */
LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)",
0, mpu.in_gyro_x_dmp_bias, getTimestamp());
if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, 0) < 0) {
LOGE("HAL:Error writing to gyro_x_dmp_bias");
return;
}
LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)",
0, mpu.in_gyro_y_dmp_bias, getTimestamp());
if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, 0) < 0) {
LOGE("HAL:Error writing to gyro_y_dmp_bias");
return;
}
LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)",
0, mpu.in_gyro_z_dmp_bias, getTimestamp());
if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, 0) < 0) {
LOGE("HAL:Error writing to gyro_z_dmp_bias");
return;
}
LOGV_IF(EXTRA_VERBOSE, "HAL:Zero Gyro DMP Calibrated Bias Applied");
return;
}
void MPLSensor::setGyroBias()
{
VFUNC_LOG;
if(mGyroBiasAvailable == false)
return;
long bias[3];
long gyroSensitivity = inv_get_gyro_sensitivity();
if(gyroSensitivity == 0) {
gyroSensitivity = mGyroScale;
}
inv_get_gyro_bias_dmp_units(bias);
/* Write to Driver */
LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
bias[0], mpu.in_gyro_x_dmp_bias, getTimestamp());
if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, bias[0]) < 0) {
LOGE("HAL:Error writing to gyro_x_dmp_bias");
return;
}
LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
bias[1], mpu.in_gyro_y_dmp_bias, getTimestamp());
if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, bias[1]) < 0) {
LOGE("HAL:Error writing to gyro_y_dmp_bias");
return;
}
LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
bias[2], mpu.in_gyro_z_dmp_bias, getTimestamp());
if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, bias[2]) < 0) {
LOGE("HAL:Error writing to gyro_z_dmp_bias");
return;
}
mGyroBiasApplied = true;
mGyroBiasAvailable = false;
LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro DMP Calibrated Bias Applied");
return;
}
void MPLSensor::getFactoryAccelBias()
{
VFUNC_LOG;
long temp;
/* Get Values from MPL */
inv_get_accel_bias(mFactoryAccelBias);
LOGV_IF(ENG_VERBOSE, "Factory Accel Bias (mg) %ld %ld %ld", mFactoryAccelBias[0], mFactoryAccelBias[1], mFactoryAccelBias[2]);
mFactoryAccelBiasAvailable = true;
return;
}
void MPLSensor::setFactoryAccelBias()
{
VFUNC_LOG;
if(mFactoryAccelBiasAvailable == false)
return;
/* add scaling here - depends on self test parameters */
int scaleRatio = mAccelScale / mAccelSelfTestScale;
int offsetScale = 16;
long tempBias;
LOGV_IF(ENG_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio);
LOGV_IF(ENG_VERBOSE, "HAL: offsetScale used =%d", offsetScale);
/* Write to Driver */
tempBias = -mFactoryAccelBias[0] / 65536.f * scaleRatio / offsetScale;
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
tempBias, mpu.in_accel_x_offset, getTimestamp());
if(write_attribute_sensor_continuous(accel_x_offset_fd, tempBias) < 0) {
LOGE("HAL:Error writing to accel_x_offset");
return;
}
tempBias = -mFactoryAccelBias[1] / 65536.f * scaleRatio / offsetScale;
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
tempBias, mpu.in_accel_y_offset, getTimestamp());
if(write_attribute_sensor_continuous(accel_y_offset_fd, tempBias) < 0) {
LOGE("HAL:Error writing to accel_y_offset");
return;
}
tempBias = -mFactoryAccelBias[2] / 65536.f * scaleRatio / offsetScale;
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
tempBias, mpu.in_accel_z_offset, getTimestamp());
if(write_attribute_sensor_continuous(accel_z_offset_fd, tempBias) < 0) {
LOGE("HAL:Error writing to accel_z_offset");
return;
}
mFactoryAccelBiasAvailable = false;
LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Accel Calibrated Bias Applied");
return;
}
void MPLSensor::getAccelBias()
{
VFUNC_LOG;
long temp;
/* Get Values from MPL */
inv_get_mpl_accel_bias(mAccelBias, &temp);
LOGV_IF(ENG_VERBOSE, "Accel Bias (mg) %ld %ld %ld",
mAccelBias[0], mAccelBias[1], mAccelBias[2]);
mAccelBiasAvailable = true;
return;
}
/* set accel bias obtained from MPL
bias is scaled by 65536 from MPL
DMP expects: bias * 536870912 / 2^30 = bias / 2 (in body frame)
*/
void MPLSensor::setAccelBias()
{
VFUNC_LOG;
if(mAccelBiasAvailable == false) {
LOGV_IF(ENG_VERBOSE, "HAL: setAccelBias - accel bias not available");
return;
}
/* write to driver */
LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
(long) (mAccelBias[0] / 65536.f / 2),
mpu.in_accel_x_dmp_bias, getTimestamp());
if(write_attribute_sensor_continuous(
accel_x_dmp_bias_fd, (long)(mAccelBias[0] / 65536.f / 2)) < 0) {
LOGE("HAL:Error writing to accel_x_dmp_bias");
return;
}
LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
(long)(mAccelBias[1] / 65536.f / 2),
mpu.in_accel_y_dmp_bias, getTimestamp());
if(write_attribute_sensor_continuous(
accel_y_dmp_bias_fd, (long)(mAccelBias[1] / 65536.f / 2)) < 0) {
LOGE("HAL:Error writing to accel_y_dmp_bias");
return;
}
LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
(long)(mAccelBias[2] / 65536 / 2),
mpu.in_accel_z_dmp_bias, getTimestamp());
if(write_attribute_sensor_continuous(
accel_z_dmp_bias_fd, (long)(mAccelBias[2] / 65536 / 2)) < 0) {
LOGE("HAL:Error writing to accel_z_dmp_bias");
return;
}
mAccelBiasAvailable = false;
mAccelBiasApplied = true;
LOGV_IF(EXTRA_VERBOSE, "HAL:Accel DMP Calibrated Bias Applied");
return;
}
int MPLSensor::isCompassDisabled(void)
{
VFUNC_LOG;
if(mCompassSensor->getFd() < 0 && !mCompassSensor->isIntegrated()) {
LOGI_IF(EXTRA_VERBOSE, "HAL: Compass is disabled, Six-axis Sensor Fusion is used.");
return 1;
}
return 0;
}
int MPLSensor::checkBatchEnabled(void)
{
VFUNC_LOG;
return ((mFeatureActiveMask & INV_DMP_BATCH_MODE)? 1:0);
}
/* precondition: framework disallows this case, ie enable continuous sensor, */
/* and enable batch sensor */
/* if one sensor is in continuous mode, HAL disallows enabling batch for this sensor */
/* or any other sensors */
int MPLSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout)
{
VFUNC_LOG;
int res = 0;
if (isMpuNonDmp())
return res;
/* Enables batch mode and sets timeout for the given sensor */
/* enum SENSORS_BATCH_DRY_RUN, SENSORS_BATCH_WAKE_UPON_FIFO_FULL */
bool dryRun = false;
android::String8 sname;
int what = -1;
int enabled_sensors = mEnabled;
int batchMode = timeout > 0 ? 1 : 0;
LOGI_IF(DEBUG_BATCHING || ENG_VERBOSE,
"HAL:batch called - handle=%d, flags=%d, period=%lld, timeout=%lld",
handle, flags, period_ns, timeout);
if(flags & SENSORS_BATCH_DRY_RUN) {
dryRun = true;
LOGI_IF(PROCESS_VERBOSE,
"HAL:batch - dry run mode is set (%d)", SENSORS_BATCH_DRY_RUN);
}
/* check if we can support issuing interrupt before FIFO fills-up */
/* in a given timeout. */
if (flags & SENSORS_BATCH_WAKE_UPON_FIFO_FULL) {
LOGE("HAL: batch SENSORS_BATCH_WAKE_UPON_FIFO_FULL is not supported");
return -EINVAL;
}
getHandle(handle, what, sname);
if(uint32_t(what) >= NumSensors) {
LOGE("HAL:batch sensors %d not found", what);
return -EINVAL;
}
LOGV_IF(PROCESS_VERBOSE,
"HAL:batch : %llu ns, (%.2f Hz)", period_ns, 1000000000.f / period_ns);
// limit all rates to reasonable ones */
if (period_ns < 5000000LL) {
period_ns = 5000000LL;
} else if (period_ns > 200000000LL) {
period_ns = 200000000LL;
}
LOGV_IF(PROCESS_VERBOSE,
"HAL:batch after applying upper and lower limit: %llu ns, (%.2f Hz)",
period_ns, 1000000000.f / period_ns);
LOGV_IF(PROCESS_VERBOSE,
"HAL:batch after applying upper and lower limit: %llu ns, (%.2f Hz)",
period_ns, 1000000000.f / period_ns);
switch (what) {
case Gyro:
case RawGyro:
case Accelerometer:
#ifdef ENABLE_PRESSURE
case Pressure:
#endif
case GameRotationVector:
case StepDetector:
LOGV_IF(PROCESS_VERBOSE, "HAL: batch - select sensor (handle %d)", handle);
break;
case MagneticField:
case RawMagneticField:
if(timeout > 0 && !mCompassSensor->isIntegrated())
return -EINVAL;
else
LOGV_IF(PROCESS_VERBOSE, "HAL: batch - select sensor (handle %d)", handle);
break;
default:
if (timeout > 0) {
LOGE("sensor (handle %d) is not supported in batch mode", handle);
return -EINVAL;
}
}
if(dryRun == true) {
LOGI("HAL: batch Dry Run is complete");
return 0;
}
if (what == StepCounter) {
mStepCountPollTime = period_ns;
LOGI("HAL: set step count poll time = %lld nS (%.2f Hz)",
mStepCountPollTime, 1000000000.f / mStepCountPollTime);
}
int tempBatch = 0;
if (timeout > 0) {
tempBatch = mBatchEnabled | (1 << what);
} else {
tempBatch = mBatchEnabled & ~(1 << what);
}
if (!computeBatchSensorMask(mEnabled, tempBatch)) {
batchMode = 0;
} else {
batchMode = 1;
}
/* get maximum possible bytes to batch per sample */
/* get minimum delay for each requested sensor */
ssize_t nBytes = 0;
int64_t wanted = 1000000000LL, ns = 0;
int64_t timeoutInMs = 0;
for (int i = 0; i < NumSensors; i++) {
if (batchMode == 1) {
ns = mBatchDelays[i];
LOGV_IF(DEBUG_BATCHING && EXTRA_VERBOSE,
"HAL:batch - requested sensor=0x%01x, batch delay=%lld", mEnabled & (1 << i), ns);
// take the min delay ==> max rate
wanted = (ns < wanted) ? ns : wanted;
if (i <= RawMagneticField) {
nBytes += 8;
}
#ifdef ENABLE_PRESSURE
if (i == Pressure) {
nBytes += 6;
}
#endif
if ((i == StepDetector) || (i == GameRotationVector)) {
nBytes += 16;
}
}
}
/* starting from code below, we will modify hardware */
/* first edit global batch mode mask */
if (!timeout) {
mBatchEnabled &= ~(1 << what);
mBatchDelays[what] = 1000000000LL;
mDelays[what] = period_ns;
mBatchTimeouts[what] = 100000000000LL;
} else {
mBatchEnabled |= (1 << what);
mBatchDelays[what] = period_ns;
mDelays[what] = period_ns;
mBatchTimeouts[what] = timeout;
}
// Check if need to change configurations
int master_enable_call = 0;
int64_t tmp_batch_timeout = 0;
bool tmp_dmp_state = 0;
int64_t tmp_gyro_rate;
int64_t tmp_accel_rate;
int64_t tmp_compass_rate;
int64_t tmp_pressure_rate;
int64_t tmp_quat_rate;
int64_t tmp_reset_rate;
bool skip_reset_data_rate = false;
if (mFirstBatchCall) {
LOGI_IF(0, "HAL: mFirstBatchCall = %d", mFirstBatchCall);
master_enable_call++;
mFirstBatchCall = 0;
}
if (mEnableCalled) {
LOGI_IF(0, "HAL: mEnableCalled = %d", mEnableCalled);
master_enable_call++;
mEnableCalled = 0;
}
if(((int)mOldBatchEnabledMask != batchMode) || batchMode) {
calcBatchTimeout(batchMode, &tmp_batch_timeout);
if (tmp_batch_timeout != mBatchTimeoutInMs)
master_enable_call++;
if (computeDmpState(&tmp_dmp_state) < 0) {
LOGE("HAL:ERR can't compute dmp state");
}
if (tmp_dmp_state != mDmpState)
master_enable_call++;
}
if (batchMode == 1) {
if (calcBatchDataRates(&tmp_gyro_rate, &tmp_accel_rate, &tmp_compass_rate, &tmp_pressure_rate, &tmp_quat_rate) < 0) {
LOGE("HAL:ERR can't get batch data rates");
}
if (tmp_gyro_rate != mGyroBatchRate)
master_enable_call++;
if (tmp_accel_rate != mAccelBatchRate)
master_enable_call++;
if (tmp_compass_rate != mCompassBatchRate)
master_enable_call++;
if (tmp_pressure_rate != mPressureBatchRate)
master_enable_call++;
if (tmp_quat_rate != mQuatBatchRate)
master_enable_call++;
} else {
if (calctDataRates(&tmp_reset_rate, &tmp_gyro_rate, &tmp_accel_rate, &tmp_compass_rate, &tmp_pressure_rate) < 0) {
skip_reset_data_rate = true;
LOGV_IF(ENG_VERBOSE, "HAL:ERR can't get output rate back to original setting");
}
if (tmp_reset_rate != mResetRate)
master_enable_call++;
if (tmp_gyro_rate != mGyroRate)
master_enable_call++;
if (tmp_accel_rate != mAccelRate)
master_enable_call++;
if (tmp_compass_rate != mCompassRate)
master_enable_call++;
if (tmp_pressure_rate != mPressureRate)
master_enable_call++;
}
uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE));
if (dataInterrupt != mDataInterrupt)
master_enable_call++;
if (master_enable_call == 0) {
LOGI_IF(0, "HAL: Skip batch configurations");
goto batch_end;
} else {
LOGI_IF(0, "HAL: Do batch configurations");
}
// reset master enable
res = masterEnable(0);
if (res < 0) {
return res;
}
if(((int)mOldBatchEnabledMask != batchMode) || batchMode) {
/* remember batch mode that is set */
mOldBatchEnabledMask = batchMode;
/* For these sensors, switch to different data output */
int featureMask = computeBatchDataOutput();
LOGV_IF(ENG_VERBOSE, "batchMode =%d, featureMask=0x%x, mEnabled=%d",
batchMode, featureMask, mEnabled);
if (DEBUG_BATCHING && EXTRA_VERBOSE) {
LOGV("HAL:batch - sensor=0x%01x", mBatchEnabled);
for (int d = 0; d < NumSensors; d++) {
LOGV("HAL:batch - sensor status=0x%01x batch status=0x%01x timeout=%lld delay=%lld",
mEnabled & (1 << d), (mBatchEnabled & (1 << d)), mBatchTimeouts[d],
mBatchDelays[d]);
}
}
/* case for Ped standalone */
if ((batchMode == 1) && (featureMask & INV_DMP_PED_STANDALONE) &&
(mFeatureActiveMask & INV_DMP_PEDOMETER)) {
LOGI_IF(ENG_VERBOSE, "batch - ID_P only = 0x%x", mBatchEnabled);
enablePedQuaternion(0);
enablePedStandalone(1);
} else {
enablePedStandalone(0);
if (featureMask & INV_DMP_PED_QUATERNION) {
enableLPQuaternion(0);
enablePedQuaternion(1);
}
}
/* case for Ped Quaternion */
if ((batchMode == 1) && (featureMask & INV_DMP_PED_QUATERNION) &&
(mEnabled & (1 << GameRotationVector)) &&
(mFeatureActiveMask & INV_DMP_PEDOMETER)) {
LOGI_IF(ENG_VERBOSE, "batch - ID_P and GRV or ALL = 0x%x", mBatchEnabled);
LOGI_IF(ENG_VERBOSE, "batch - ID_P is enabled for batching, PED quat will be automatically enabled");
enableLPQuaternion(0);
enablePedQuaternion(1);
/* set pedq rate */
wanted = mBatchDelays[GameRotationVector];
setPedQuaternionRate(wanted);
} else if (!(featureMask & INV_DMP_PED_STANDALONE)){
LOGV_IF(ENG_VERBOSE, "batch - PedQ Toggle back to normal 6 axis");
if (mEnabled & (1 << GameRotationVector)) {
enableLPQuaternion(checkLPQRateSupported());
}
enablePedQuaternion(0);
} else {
enablePedQuaternion(0);
}
/* case for Ped indicator */
if ((batchMode == 1) && ((featureMask & INV_DMP_PED_INDICATOR))) {
enablePedIndicator(1);
} else {
enablePedIndicator(0);
}
/* case for Six Axis Quaternion */
if ((batchMode == 1) && (featureMask & INV_DMP_6AXIS_QUATERNION) &&
(mEnabled & (1 << GameRotationVector))) {
LOGI_IF(ENG_VERBOSE, "batch - GRV = 0x%x", mBatchEnabled);
enableLPQuaternion(0);
enable6AxisQuaternion(1);
if (what == GameRotationVector) {
setInitial6QuatValue();
}
/* set sixaxis rate */
wanted = mBatchDelays[GameRotationVector];
set6AxisQuaternionRate(wanted);
} else if (!(featureMask & INV_DMP_PED_QUATERNION)){
LOGV_IF(ENG_VERBOSE, "batch - 6Axis Toggle back to normal 6 axis");
if (mEnabled & (1 << GameRotationVector)) {
enableLPQuaternion(checkLPQRateSupported());
}
enable6AxisQuaternion(0);
} else {
enable6AxisQuaternion(0);
}
/* TODO: This may make a come back some day */
/* Not to overflow hardware FIFO if flag is set */
/*if (flags & (1 << SENSORS_BATCH_WAKE_UPON_FIFO_FULL)) {
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
0, mpu.batchmode_wake_fifo_full_on, getTimestamp());
if (write_sysfs_int(mpu.batchmode_wake_fifo_full_on, 0) < 0) {
LOGE("HAL:ERR can't write batchmode_wake_fifo_full_on");
}
}*/
writeBatchTimeout(batchMode, tmp_batch_timeout);
if (SetDmpState(tmp_dmp_state) < 0) {
LOGE("HAL:ERR can't set dmp state");
}
}//end of batch mode modify
if (batchMode == 1) {
/* set batch rates */
if (setBatchDataRates(tmp_gyro_rate, tmp_accel_rate, tmp_compass_rate, tmp_pressure_rate, tmp_quat_rate) < 0) {
LOGE("HAL:ERR can't set batch data rates");
}
} else {
/* reset sensor rate */
if (!skip_reset_data_rate) {
if (resetDataRates(tmp_reset_rate, tmp_gyro_rate, tmp_accel_rate, tmp_compass_rate, tmp_pressure_rate) < 0) {
LOGE("HAL:ERR can't reset output rate back to original setting");
}
}
}
// set sensor data interrupt
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");
}
mDataInterrupt = dataInterrupt;
if (enabled_sensors || mFeatureActiveMask) {
masterEnable(1);
}
batch_end:
return res;
}
/* Send empty event when: */
/* 1. batch mode is not enabled */
/* 2. no data in HW FIFO */
/* return status zero if (2) */
int MPLSensor::flush(int handle)
{
VFUNC_LOG;
int res = 0;
int status = 0;
android::String8 sname;
int what = -1;
getHandle(handle, what, sname);
if (uint32_t(what) >= NumSensors) {
LOGE("HAL:flush - what=%d is invalid", what);
return -EINVAL;
}
LOGV_IF(PROCESS_VERBOSE, "HAL: flush - select sensor %s (handle %d)", sname.string(), handle);
if (((what != StepDetector) && (!(mEnabled & (1 << what)))) ||
((what == StepDetector) && !(mFeatureActiveMask & INV_DMP_PEDOMETER))) {
LOGV_IF(ENG_VERBOSE, "HAL: flush - sensor %s not enabled", sname.string());
return -EINVAL;
}
if(!(mBatchEnabled & (1 << what))) {
LOGV_IF(PROCESS_VERBOSE, "HAL:flush - batch mode not enabled for sensor %s (handle %d)", sname.string(), handle);
}
mFlushSensorEnabledVector.push_back(handle);
/*write sysfs */
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
mpu.flush_batch, getTimestamp());
status = read_sysfs_int(mpu.flush_batch, &res);
if (status < 0)
LOGE("HAL: flush - error invoking flush_batch");
/* driver returns 0 if FIFO is empty */
if (res == 0) {
LOGV_IF(ENG_VERBOSE, "HAL: flush - no data in FIFO");
}
LOGV_IF(ENG_VERBOSE, "HAl:flush - mFlushSensorEnabledVector=%d res=%d status=%d", handle, res, status);
return 0;
}
int MPLSensor::selectAndSetQuaternion(int batchMode, int mEnabled, long long featureMask)
{
VFUNC_LOG;
int res = 0;
int64_t wanted;
/* case for Ped Quaternion */
if (batchMode == 1) {
if ((featureMask & INV_DMP_PED_QUATERNION) &&
(mEnabled & (1 << GameRotationVector)) &&
(mFeatureActiveMask & INV_DMP_PEDOMETER)) {
enableLPQuaternion(0);
enable6AxisQuaternion(0);
setInitial6QuatValue();
enablePedQuaternion(1);
/* set pedq rate */
wanted = mBatchDelays[GameRotationVector];
setPedQuaternionRate(wanted);
} else if ((featureMask & INV_DMP_6AXIS_QUATERNION) &&
(mEnabled & (1 << GameRotationVector))) {
enableLPQuaternion(0);
enablePedQuaternion(0);
setInitial6QuatValue();
enable6AxisQuaternion(1);
/* set sixaxis rate */
wanted = mBatchDelays[GameRotationVector];
set6AxisQuaternionRate(wanted);
} else {
enablePedQuaternion(0);
enable6AxisQuaternion(0);
}
} else {
if(mEnabled & (1 << GameRotationVector)) {
enablePedQuaternion(0);
enable6AxisQuaternion(0);
enableLPQuaternion(checkLPQRateSupported());
}
else {
enablePedQuaternion(0);
enable6AxisQuaternion(0);
}
}
return res;
}
/*
Select Quaternion and Options for Batching
ID_P ID_GRV HW Batch Type
a 1 1 1 PedQ, Ped Indicator, HW
b 1 1 0 PedQ
c 1 0 1 Ped Indicator, HW
d 1 0 0 Ped Standalone, Ped Indicator
e 0 1 1 6Q, HW
f 0 1 0 6Q
g 0 0 1 HW
h 0 0 0 LPQ <defualt>
*/
int MPLSensor::computeBatchDataOutput()
{
VFUNC_LOG;
int featureMask = 0;
if (mBatchEnabled == 0)
return 0;//h
uint32_t hardwareSensorMask = (1 << Gyro)
| (1 << RawGyro)
| (1 << Accelerometer)
| (1 << MagneticField)
#ifdef ENABLE_PRESSURE
| (1 << Pressure)
#endif
| (1 << RawMagneticField);
LOGV_IF(ENG_VERBOSE, "hardwareSensorMask = 0x%0x, mBatchEnabled = 0x%0x",
hardwareSensorMask, mBatchEnabled);
if (mBatchEnabled & (1 << StepDetector)) {
if (mBatchEnabled & (1 << GameRotationVector)) {
if ((mBatchEnabled & hardwareSensorMask)) {
featureMask |= INV_DMP_6AXIS_QUATERNION;//a
featureMask |= INV_DMP_PED_INDICATOR;
//LOGE("batch output: a");
} else {
featureMask |= INV_DMP_PED_QUATERNION; //b
featureMask |= INV_DMP_PED_INDICATOR; //always piggy back a bit
//LOGE("batch output: b");
}
} else {
if (mBatchEnabled & hardwareSensorMask) {
featureMask |= INV_DMP_PED_INDICATOR; //c
//LOGE("batch output: c");
} else {
featureMask |= INV_DMP_PED_STANDALONE; //d
featureMask |= INV_DMP_PED_INDICATOR; //required for standalone
//LOGE("batch output: d");
}
}
} else if (mBatchEnabled & (1 << GameRotationVector)) {
featureMask |= INV_DMP_6AXIS_QUATERNION; //e,f
//LOGE("batch output: e,f");
} else {
LOGV_IF(ENG_VERBOSE,
"HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask);
//LOGE("batch output: g");
return 0; //g
}
LOGV_IF(ENG_VERBOSE,
"HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask);
return featureMask;
}
int MPLSensor::getDmpPedometerFd()
{
VFUNC_LOG;
LOGV_IF(EXTRA_VERBOSE, "getDmpPedometerFd returning %d", dmp_pedometer_fd);
return dmp_pedometer_fd;
}
/* @param [in] : outputType = 1 --event is from PED_Q */
/* outputType = 0 --event is from ID_SC, ID_P */
int MPLSensor::readDmpPedometerEvents(sensors_event_t* data, int count,
int32_t id, int outputType)
{
VFUNC_LOG;
int res = 0;
char dummy[4];
int numEventReceived = 0;
int update = 0;
LOGI_IF(0, "HAL: Read Pedometer Event ID=%d", id);
switch (id) {
case ID_P:
if (mDmpPedometerEnabled && count > 0) {
/* Handles return event */
LOGI("HAL: Step detected");
update = sdHandler(&mSdEvents);
}
if (update && count > 0) {
*data++ = mSdEvents;
count--;
numEventReceived++;
}
break;
case ID_SC:
FILE *fp;
uint64_t stepCount;
uint64_t stepCountTs;
if (mDmpStepCountEnabled && count > 0) {
fp = fopen(mpu.pedometer_steps, "r");
if (fp == NULL) {
LOGE("HAL:cannot open pedometer_steps");
} else {
if (fscanf(fp, "%lld\n", &stepCount) < 0) {
LOGV_IF(PROCESS_VERBOSE, "HAL:cannot read pedometer_steps");
if (fclose(fp) < 0) {
LOGW("HAL:cannot close pedometer_steps");
}
return 0;
}
if (fclose(fp) < 0) {
LOGW("HAL:cannot close pedometer_steps");
}
}
/* return event onChange only */
if (stepCount == mLastStepCount) {
return 0;
}
mLastStepCount = stepCount;
/* Read step count timestamp */
fp = fopen(mpu.pedometer_counter, "r");
if (fp == NULL) {
LOGE("HAL:cannot open pedometer_counter");
} else{
if (fscanf(fp, "%lld\n", &stepCountTs) < 0) {
LOGE("HAL:cannot read pedometer_counter");
if (fclose(fp) < 0) {
LOGE("HAL:cannot close pedometer_counter");
}
return 0;
}
if (fclose(fp) < 0) {
LOGE("HAL:cannot close pedometer_counter");
return 0;
}
}
mScEvents.timestamp = stepCountTs;
/* Handles return event */
update = scHandler(&mScEvents);
}
if (update && count > 0) {
*data++ = mScEvents;
count--;
numEventReceived++;
}
break;
}
if (!outputType) {
// read dummy data per driver's request
// only required if actual irq is issued
read(dmp_pedometer_fd, dummy, 4);
} else {
return 1;
}
return numEventReceived;
}
int MPLSensor::getDmpSignificantMotionFd()
{
VFUNC_LOG;
LOGV_IF(EXTRA_VERBOSE, "getDmpSignificantMotionFd returning %d",
dmp_sign_motion_fd);
return dmp_sign_motion_fd;
}
int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count)
{
VFUNC_LOG;
int res = 0;
char dummy[4];
int vibrator = 0;
FILE *fp;
int sensors = mEnabled;
int numEventReceived = 0;
int update = 0;
static int64_t lastVibTrigger = 0;
if (mDmpSignificantMotionEnabled && count > 0) {
// If vibrator is going off, ignore this event
fp = fopen(VIBRATOR_ENABLE_FILE, "r");
if (fp != NULL) {
if (fscanf(fp, "%d\n", &vibrator) < 0) {
LOGE("HAL:cannot read %s", VIBRATOR_ENABLE_FILE);
}
if (fclose(fp) < 0) {
LOGE("HAL:cannot close %s", VIBRATOR_ENABLE_FILE);
}
if (vibrator != 0) {
lastVibTrigger = android::elapsedRealtimeNano();
LOGV_IF(ENG_VERBOSE, "SMD triggered by vibrator, ignoring SMD event");
return 0;
} else if (lastVibTrigger) {
// vibrator recently triggered SMD, discard related events
int64_t now = android::elapsedRealtimeNano();
if ((now - lastVibTrigger) < MIN_TRIGGER_TIME_AFTER_VIBRATOR_NS) {
LOGV_IF(ENG_VERBOSE, "HAL: SMD triggered too close to vibrator (delta %lldnS), ignoring",
(now-lastVibTrigger));
return 0;
} else {
LOGV_IF(ENG_VERBOSE, "HAL: SMD triggered %lld after vibrator (last %lld now %lld)",
now-lastVibTrigger, lastVibTrigger, now);
lastVibTrigger = 0;
}
}
} else {
LOGE("HAL:cannot open %s", VIBRATOR_ENABLE_FILE);
}
/* By implementation, smd is disabled once an event is triggered */
sensors_event_t temp;
/* Handles return event */
LOGI("HAL: SMD detected");
int update = smHandler(&mSmEvents);
if (update && count > 0) {
*data++ = mSmEvents;
count--;
numEventReceived++;
/* reset smd state */
mDmpSignificantMotionEnabled = 0;
mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION;
/* auto disable this sensor */
enableDmpSignificantMotion(0);
}
}
// read dummy data per driver's request
read(dmp_sign_motion_fd, dummy, 4);
return numEventReceived;
}
int MPLSensor::enableDmpSignificantMotion(int en)
{
VFUNC_LOG;
int res = 0;
int enabled_sensors = mEnabled;
if (isMpuNonDmp())
return res;
// reset master enable
res = masterEnable(0);
if (res < 0)
return res;
//Toggle significant montion detection
if(en) {
LOGV_IF(ENG_VERBOSE, "HAL:Enabling Significant Motion");
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1, mpu.smd_enable, getTimestamp());
if (write_sysfs_int(mpu.smd_enable, 1) < 0) {
LOGE("HAL:ERR can't write DMP smd_enable");
res = -1; //Indicate an err
}
mFeatureActiveMask |= INV_DMP_SIGNIFICANT_MOTION;
}
else {
LOGV_IF(ENG_VERBOSE, "HAL:Disabling Significant Motion");
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
0, mpu.smd_enable, getTimestamp());
if (write_sysfs_int(mpu.smd_enable, 0) < 0) {
LOGE("HAL:ERR write DMP smd_enable");
}
mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION;
}
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;
}
void MPLSensor::setInitial6QuatValue()
{
VFUNC_LOG;
if (!mInitial6QuatValueAvailable)
return;
/* convert to unsigned char array */
size_t length = 16;
unsigned char quat[16];
convert_long_to_hex_char(mInitial6QuatValue, quat, 4);
/* write to sysfs */
LOGV_IF(EXTRA_VERBOSE, "HAL:sysfs:echo quat value > %s", mpu.six_axis_q_value);
LOGV_IF(EXTRA_VERBOSE && ENG_VERBOSE, "quat=%ld,%ld,%ld,%ld", mInitial6QuatValue[0],
mInitial6QuatValue[1],
mInitial6QuatValue[2],
mInitial6QuatValue[3]);
FILE* fptr = fopen(mpu.six_axis_q_value, "w");
if(fptr == NULL) {
LOGE("HAL:could not open six_axis_q_value");
} else {
if (fwrite(quat, 1, length, fptr) != length) {
LOGE("HAL:write six axis q value failed");
} else {
mInitial6QuatValueAvailable = 0;
}
if (fclose(fptr) < 0) {
LOGE("HAL:could not close six_axis_q_value");
}
}
return;
}
int MPLSensor::writeSignificantMotionParams(bool toggleEnable,
uint32_t delayThreshold1,
uint32_t delayThreshold2,
uint32_t motionThreshold)
{
VFUNC_LOG;
int res = 0;
// Turn off enable
if (toggleEnable) {
masterEnable(0);
}
// Write supplied values
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
delayThreshold1, mpu.smd_delay_threshold, getTimestamp());
res = write_sysfs_int(mpu.smd_delay_threshold, delayThreshold1);
if (res == 0) {
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
delayThreshold2, mpu.smd_delay_threshold2, getTimestamp());
res = write_sysfs_int(mpu.smd_delay_threshold2, delayThreshold2);
}
if (res == 0) {
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
motionThreshold, mpu.smd_threshold, getTimestamp());
res = write_sysfs_int(mpu.smd_threshold, motionThreshold);
}
// Turn on enable
if (toggleEnable) {
masterEnable(1);
}
return res;
}
int MPLSensor::calcBatchDataRates(int64_t *gyro_rate, int64_t *accel_rate, int64_t *compass_rate, int64_t *pressure_rate, int64_t *quat_rate)
{
VFUNC_LOG;
int res = 0;
int tempFd = -1;
int64_t gyroRate;
int64_t accelRate;
int64_t compassRate;
#ifdef ENABLE_PRESSURE
int64_t pressureRate;
#endif
int64_t quatRate = 0;
int mplGyroRate;
int mplAccelRate;
int mplCompassRate;
int mplQuatRate;
#ifdef ENABLE_MULTI_RATE
gyroRate = mBatchDelays[Gyro];
/* take care of case where only one type of gyro sensors or
compass sensors is turned on */
if (mBatchEnabled & (1 << Gyro) || mBatchEnabled & (1 << RawGyro)) {
gyroRate = (mBatchDelays[Gyro] <= mBatchDelays[RawGyro]) ?
(mBatchEnabled & (1 << Gyro) ? mBatchDelays[Gyro] : mBatchDelays[RawGyro]):
(mBatchEnabled & (1 << RawGyro) ? mBatchDelays[RawGyro] : mBatchDelays[Gyro]);
}
compassRate = mBatchDelays[MagneticField];
if (mBatchEnabled & (1 << MagneticField) || mBatchEnabled & (1 << RawMagneticField)) {
compassRate = (mBatchDelays[MagneticField] <= mBatchDelays[RawMagneticField]) ?
(mBatchEnabled & (1 << MagneticField) ? mBatchDelays[MagneticField] :
mBatchDelays[RawMagneticField]) :
(mBatchEnabled & (1 << RawMagneticField) ? mBatchDelays[RawMagneticField] :
mBatchDelays[MagneticField]);
}
accelRate = mBatchDelays[Accelerometer];
#ifdef ENABLE_PRESSURE
pressureRate = mBatchDelays[Pressure];
#endif //ENABLE_PRESSURE
if ((mFeatureActiveMask & INV_DMP_PED_QUATERNION) ||
(mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)) {
quatRate = mBatchDelays[GameRotationVector];
mplQuatRate = (int) quatRate / 1000LL;
inv_set_quat_sample_rate(mplQuatRate);
inv_set_rotation_vector_6_axis_sample_rate(mplQuatRate);
LOGV_IF(PROCESS_VERBOSE,
"HAL:MPL rv 6 axis sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplQuatRate,
1000000000.f / quatRate );
LOGV_IF(PROCESS_VERBOSE,
"HAL:MPL quat sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplQuatRate,
1000000000.f / quatRate );
//getDmpRate(&quatRate);
}
mplGyroRate = (int) gyroRate / 1000LL;
mplAccelRate = (int) accelRate / 1000LL;
mplCompassRate = (int) compassRate / 1000LL;
/* set rate in MPL */
/* compass can only do 100Hz max */
inv_set_gyro_sample_rate(mplGyroRate);
inv_set_accel_sample_rate(mplAccelRate);
inv_set_compass_sample_rate(mplCompassRate);
LOGV_IF(PROCESS_VERBOSE,
"HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplGyroRate, 1000000000.f / gyroRate);
LOGV_IF(PROCESS_VERBOSE,
"HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplAccelRate, 1000000000.f / accelRate);
LOGV_IF(PROCESS_VERBOSE,
"HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplCompassRate, 1000000000.f / compassRate);
#else
/* search the minimum delay requested across all enabled sensors */
int64_t wanted = 1000000000LL;
for (int i = 0; i < NumSensors; i++) {
if (mBatchEnabled & (1 << i)) {
int64_t ns = mBatchDelays[i];
wanted = wanted < ns ? wanted : ns;
}
}
gyroRate = wanted;
accelRate = wanted;
compassRate = wanted;
#ifdef ENABLE_PRESSURE
pressureRate = wanted;
#endif
#endif
*gyro_rate = gyroRate;
*accel_rate = accelRate;
*compass_rate = compassRate;
#ifdef ENABLE_PRESSURE
*pressure_rate = pressureRate;
#endif
*quat_rate = quatRate;
return 0;
}
int MPLSensor::MPLSensor::setBatchDataRates(int64_t gyroRate, int64_t accelRate, int64_t compassRate, int64_t pressureRate, int64_t quatRate)
{
VFUNC_LOG;
int res = 0;
int tempFd = -1;
if ((mFeatureActiveMask & INV_DMP_PED_QUATERNION) ||
(mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)) {
getDmpRate(&quatRate);
}
/* takes care of gyro rate */
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / gyroRate, mpu.gyro_rate,
getTimestamp());
tempFd = open(mpu.gyro_rate, O_RDWR);
res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
if(res < 0) {
LOGE("HAL:GYRO update delay error");
}
/* takes care of accel rate */
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / accelRate, mpu.accel_rate,
getTimestamp());
tempFd = open(mpu.accel_rate, O_RDWR);
res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
LOGE_IF(res < 0, "HAL:ACCEL update delay error");
/* takes care of compass rate */
if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
compassRate = mCompassSensor->getMinDelay() * 1000LL;
}
mCompassSensor->setDelay(ID_M, compassRate);
#ifdef ENABLE_PRESSURE
/* takes care of pressure rate */
mPressureSensor->setDelay(ID_PS, pressureRate);
#endif
mGyroBatchRate = gyroRate;
mAccelBatchRate = accelRate;
mCompassBatchRate = compassRate;
mPressureBatchRate = pressureRate;
mQuatBatchRate = quatRate;
return res;
}
/* set batch data rate */
/* this function should be optimized */
int MPLSensor::setBatchDataRates()
{
VFUNC_LOG;
int res = 0;
int64_t gyroRate;
int64_t accelRate;
int64_t compassRate;
int64_t pressureRate;
int64_t quatRate;
calcBatchDataRates(&gyroRate, &accelRate, &compassRate, &pressureRate, &quatRate);
setBatchDataRates(gyroRate, accelRate, compassRate, pressureRate, quatRate);
return res;
}
int MPLSensor::calctDataRates(int64_t *resetRate, int64_t *gyroRate, int64_t *accelRate, int64_t *compassRate, int64_t *pressureRate)
{
VFUNC_LOG;
int res = 0;
int tempFd = -1;
int64_t wanted = 1000000000LL;
if (!mEnabled) {
LOGV_IF(ENG_VERBOSE, "skip resetDataRates");
return -1;
}
/* search the minimum delay requested across all enabled sensors */
/* skip setting rates if it is not changed */
for (int i = 0; i < NumSensors; i++) {
if (mEnabled & (1 << i)) {
int64_t ns = mDelays[i];
#ifdef ENABLE_PRESSURE
if ((wanted == ns) && (i != Pressure)) {
LOGV_IF(ENG_VERBOSE, "skip resetDataRates : same delay mDelays[%d]=%lld", i,mDelays[i]);
//return 0;
}
#endif
LOGV_IF(ENG_VERBOSE, "resetDataRates - mDelays[%d]=%lld", i, mDelays[i]);
wanted = wanted < ns ? wanted : ns;
}
}
*resetRate = wanted;
*gyroRate = wanted;
*accelRate = wanted;
*compassRate = wanted;
*pressureRate = wanted;
return 0;
}
int MPLSensor::resetDataRates(int64_t resetRate, int64_t gyroRate, int64_t accelRate, int64_t compassRate, int64_t pressureRate)
{
VFUNC_LOG;
int res = 0;
int tempFd = -1;
int64_t wanted;
wanted = resetRate;
/* set mpl data rate */
inv_set_gyro_sample_rate((int)gyroRate/1000LL);
inv_set_accel_sample_rate((int)accelRate/1000LL);
inv_set_compass_sample_rate((int)compassRate/1000LL);
inv_set_linear_acceleration_sample_rate((int)resetRate/1000LL);
inv_set_orientation_sample_rate((int)resetRate/1000LL);
inv_set_rotation_vector_sample_rate((int)resetRate/1000LL);
inv_set_gravity_sample_rate((int)resetRate/1000LL);
inv_set_orientation_geomagnetic_sample_rate((int)resetRate/1000LL);
inv_set_rotation_vector_6_axis_sample_rate((int)resetRate/1000LL);
inv_set_geomagnetic_rotation_vector_sample_rate((int)resetRate/1000LL);
LOGV_IF(PROCESS_VERBOSE,
"HAL:MPL gyro sample rate: (mpl)=%lld us (mpu)=%.2f Hz",
gyroRate/1000LL, 1000000000.f / gyroRate);
LOGV_IF(PROCESS_VERBOSE,
"HAL:MPL accel sample rate: (mpl)=%lld us (mpu)=%.2f Hz",
accelRate/1000LL, 1000000000.f / accelRate);
LOGV_IF(PROCESS_VERBOSE,
"HAL:MPL compass sample rate: (mpl)=%lld us (mpu)=%.2f Hz",
compassRate/1000LL, 1000000000.f / compassRate);
/* reset dmp rate */
getDmpRate (&wanted);
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / wanted, mpu.gyro_fifo_rate,
getTimestamp());
tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
LOGE_IF(res < 0, "HAL:sampling frequency update delay error");
/* takes care of gyro rate */
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / gyroRate, mpu.gyro_rate,
getTimestamp());
tempFd = open(mpu.gyro_rate, O_RDWR);
res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
if(res < 0) {
LOGE("HAL:GYRO update delay error");
}
/* takes care of accel rate */
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / accelRate, mpu.accel_rate,
getTimestamp());
tempFd = open(mpu.accel_rate, O_RDWR);
res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
LOGE_IF(res < 0, "HAL:ACCEL update delay error");
/* takes care of compass rate */
if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
compassRate = mCompassSensor->getMinDelay() * 1000LL;
}
mCompassSensor->setDelay(ID_M, compassRate);
#ifdef ENABLE_PRESSURE
/* takes care of pressure rate */
mPressureSensor->setDelay(ID_PS, pressureRate);
#endif
/* takes care of lpq case for data rate at 200Hz */
if (checkLPQuaternion()) {
if (resetRate <= RATE_200HZ) {
#ifndef USE_LPQ_AT_FASTEST
enableLPQuaternion(0);
#endif
}
}
mResetRate = resetRate;
mGyroRate = gyroRate;
mAccelRate = accelRate;
mCompassRate = compassRate;
mPressureRate = pressureRate;
return res;
}
/* Set sensor rate */
/* this function should be optimized */
int MPLSensor::resetDataRates()
{
VFUNC_LOG;
int res = 0;
int64_t resetRate;
int64_t gyroRate;
int64_t accelRate;
int64_t compassRate;
int64_t pressureRate;
res = calctDataRates(&resetRate, &gyroRate, &accelRate, &compassRate, &pressureRate);
if (res)
return 0;
resetDataRates(resetRate, gyroRate, accelRate, compassRate, pressureRate);
return res;
}
void MPLSensor::resetMplStates()
{
VFUNC_LOG;
LOGV_IF(ENG_VERBOSE, "HAL:resetMplStates()");
inv_gyro_was_turned_off();
inv_accel_was_turned_off();
inv_compass_was_turned_off();
inv_quaternion_sensor_was_turned_off();
return;
}
void MPLSensor::initBias()
{
VFUNC_LOG;
LOGV_IF(ENG_VERBOSE, "HAL:inititalize dmp and device offsets to 0");
if(write_attribute_sensor_continuous(accel_x_dmp_bias_fd, 0) < 0) {
LOGE("HAL:Error writing to accel_x_dmp_bias");
}
if(write_attribute_sensor_continuous(accel_y_dmp_bias_fd, 0) < 0) {
LOGE("HAL:Error writing to accel_y_dmp_bias");
}
if(write_attribute_sensor_continuous(accel_z_dmp_bias_fd, 0) < 0) {
LOGE("HAL:Error writing to accel_z_dmp_bias");
}
if(write_attribute_sensor_continuous(accel_x_offset_fd, 0) < 0) {
LOGE("HAL:Error writing to accel_x_offset");
}
if(write_attribute_sensor_continuous(accel_y_offset_fd, 0) < 0) {
LOGE("HAL:Error writing to accel_y_offset");
}
if(write_attribute_sensor_continuous(accel_z_offset_fd, 0) < 0) {
LOGE("HAL:Error writing to accel_z_offset");
}
if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, 0) < 0) {
LOGE("HAL:Error writing to gyro_x_dmp_bias");
}
if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, 0) < 0) {
LOGE("HAL:Error writing to gyro_y_dmp_bias");
}
if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, 0) < 0) {
LOGE("HAL:Error writing to gyro_z_dmp_bias");
}
if(write_attribute_sensor_continuous(gyro_x_offset_fd, 0) < 0) {
LOGE("HAL:Error writing to gyro_x_offset");
}
if(write_attribute_sensor_continuous(gyro_y_offset_fd, 0) < 0) {
LOGE("HAL:Error writing to gyro_y_offset");
}
if(write_attribute_sensor_continuous(gyro_z_offset_fd, 0) < 0) {
LOGE("HAL:Error writing to gyro_z_offset");
}
return;
}
/*TODO: reg_dump in a separate file*/
void MPLSensor::sys_dump(bool fileMode)
{
VFUNC_LOG;
char sysfs_path[MAX_SYSFS_NAME_LEN];
char scan_element_path[MAX_SYSFS_NAME_LEN];
memset(sysfs_path, 0, sizeof(sysfs_path));
memset(scan_element_path, 0, sizeof(scan_element_path));
inv_get_sysfs_path(sysfs_path);
sprintf(scan_element_path, "%s%s", sysfs_path, "/scan_elements");
read_sysfs_dir(fileMode, sysfs_path);
read_sysfs_dir(fileMode, scan_element_path);
dump_dmp_img("/data/local/read_img.h");
return;
}