diff options
Diffstat (limited to '60xx/libsensors_iio/MPLSensor.cpp')
| -rw-r--r-- | 60xx/libsensors_iio/MPLSensor.cpp | 2764 |
1 files changed, 0 insertions, 2764 deletions
diff --git a/60xx/libsensors_iio/MPLSensor.cpp b/60xx/libsensors_iio/MPLSensor.cpp deleted file mode 100644 index 031ae6e..0000000 --- a/60xx/libsensors_iio/MPLSensor.cpp +++ /dev/null @@ -1,2764 +0,0 @@ -/* -* Copyright (C) 2012 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/String8.h> -#include <string.h> -#include <linux/input.h> -#include <utils/Atomic.h> - -#include "MPLSensor.h" -#include "MPLSupport.h" -#include "sensor_params.h" -#include "local_log_def.h" - -#include "invensense.h" -#include "invensense_adv.h" -#include "ml_stored_data.h" -#include "ml_load_dmp.h" -#include "ml_sysfs_helper.h" - -// #define TESTING - -#ifdef THIRD_PARTY_ACCEL -# warning "Third party accel" -# define USE_THIRD_PARTY_ACCEL (1) -#else -# define USE_THIRD_PARTY_ACCEL (0) -#endif - -#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*)) - -/******************************************************************************/ -/* MPL interface misc. */ -/******************************************************************************/ -static int hertz_request = 200; - -#define DEFAULT_MPL_GYRO_RATE (20000L) //us -#define DEFAULT_MPL_COMPASS_RATE (20000L) //us - -#define DEFAULT_HW_GYRO_RATE (100) //Hz -#define DEFAULT_HW_ACCEL_RATE (20) //ms -#define DEFAULT_HW_COMPASS_RATE (20000000L) //ns -#define DEFAULT_HW_AKMD_COMPASS_RATE (200000000L) //ns - -/* convert ns to hardware units */ -#define HW_GYRO_RATE_NS (1000000000LL / rate_request) // to Hz -#define HW_ACCEL_RATE_NS (rate_request / (1000000L)) // to ms -#define HW_COMPASS_RATE_NS (rate_request) // to ns - -/* convert Hz to hardware units */ -#define HW_GYRO_RATE_HZ (hertz_request) -#define HW_ACCEL_RATE_HZ (1000 / hertz_request) -#define HW_COMPASS_RATE_HZ (1000000000LL / hertz_request) - -#define RATE_200HZ 5000000LL -#define RATE_15HZ 66667000LL -#define RATE_5HZ 200000000LL - -static struct sensor_t sSensorList[] = -{ - {"MPL Gyroscope", "Invensense", 1, - SENSORS_GYROSCOPE_HANDLE, - SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Raw Gyroscope", "Invensense", 1, - SENSORS_RAW_GYROSCOPE_HANDLE, - SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Accelerometer", "Invensense", 1, - SENSORS_ACCELERATION_HANDLE, - SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Magnetic Field", "Invensense", 1, - SENSORS_MAGNETIC_FIELD_HANDLE, - SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Orientation", "Invensense", 1, - SENSORS_ORIENTATION_HANDLE, - SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Rotation Vector", "Invensense", 1, - SENSORS_ROTATION_VECTOR_HANDLE, - SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Linear Acceleration", "Invensense", 1, - SENSORS_LINEAR_ACCEL_HANDLE, - SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - {"MPL Gravity", "Invensense", 1, - SENSORS_GRAVITY_HANDLE, - SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, - -#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION - {"MPL Screen Orientation", "Invensense ", 1, - SENSORS_SCREEN_ORIENTATION_HANDLE, - SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, 0, 0, 0, 0, {}}, -#endif -}; - -MPLSensor *MPLSensor::gMPLSensor = NULL; - -extern "C" { -void procData_cb_wrapper() -{ - if(MPLSensor::gMPLSensor) { - MPLSensor::gMPLSensor->cbProcData(); - } -} - -void setCallbackObject(MPLSensor* gbpt) -{ - MPLSensor::gMPLSensor = gbpt; -} - -MPLSensor* getCallbackObject() { - return MPLSensor::gMPLSensor; -} - -} // end of extern C - -#ifdef INV_PLAYBACK_DBG -static FILE *logfile = NULL; -#endif - -pthread_mutex_t GlobalHalMutex = PTHREAD_MUTEX_INITIALIZER; - -/******************************************************************************* - * MPLSensor class implementation - ******************************************************************************/ - -MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *)) - : SensorBase(NULL, NULL), - mNewData(0), - mMasterSensorMask(INV_ALL_SENSORS), - mLocalSensorMask(0), - mPollTime(-1), - mHaveGoodMpuCal(0), - mGyroAccuracy(0), - mAccelAccuracy(0), - mCompassAccuracy(0), - mSampleCount(0), - dmp_orient_fd(-1), - mDmpOrientationEnabled(0), - mEnabled(0), - mOldEnabledMask(0), - mAccelInputReader(4), - mGyroInputReader(32), - mTempScale(0), - mTempOffset(0), - mTempCurrentTime(0), - mAccelScale(2), - mPendingMask(0), - mSensorMask(0), - mFeatureActiveMask(0) { - VFUNC_LOG; - - inv_error_t rv; - int fd; - char *ver_str; - - mCompassSensor = compass; - - LOGV_IF(EXTRA_VERBOSE, - "HAL:MPLSensor constructor : numSensors = %d", numSensors); - - pthread_mutex_init(&mMplMutex, NULL); - pthread_mutex_init(&mHALMutex, NULL); - memset(mGyroOrientation, 0, sizeof(mGyroOrientation)); - memset(mAccelOrientation, 0, sizeof(mAccelOrientation)); - -#ifdef INV_PLAYBACK_DBG - LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging"); - logfile = fopen("/data/playback.bin", "wb"); - if (logfile) - inv_turn_on_data_logging(logfile); -#endif - - /* 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(); - - /* turn on power state */ - onPower(1); - - /* reset driver master enable */ - masterEnable(0); - - if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) { - /* Load DMP image if capable, ie. MPU6xxx/9xxx */ - 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 accel FSR to calcuate accel scale later */ - if (!USE_THIRD_PARTY_ACCEL) { - 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); - } - } - - /* initialize sensor data */ - memset(mPendingEvents, 0, sizeof(mPendingEvents)); - - mPendingEvents[RotationVector].version = sizeof(sensors_event_t); - mPendingEvents[RotationVector].sensor = ID_RV; - mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; - - mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); - mPendingEvents[LinearAccel].sensor = ID_LA; - mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; - - mPendingEvents[Gravity].version = sizeof(sensors_event_t); - mPendingEvents[Gravity].sensor = ID_GR; - mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; - - mPendingEvents[Gyro].version = sizeof(sensors_event_t); - mPendingEvents[Gyro].sensor = ID_GY; - mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; - - mPendingEvents[RawGyro].version = sizeof(sensors_event_t); - mPendingEvents[RawGyro].sensor = ID_RG; - mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE; - - mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); - mPendingEvents[Accelerometer].sensor = ID_A; - mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; - - /* 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[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; - - mHandlers[RotationVector] = &MPLSensor::rvHandler; - 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[Orientation] = &MPLSensor::orienHandler; - - for (int i = 0; i < numSensors; i++) { - mDelays[i] = 0; - } - - (void)inv_get_version(&ver_str); - LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str); - - /* setup MPL */ - inv_constructor_init(); - - /* 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"); - else - LOGE("HAL:Could not open or load MPL calibration file (%d)", rv); - - /* Takes external Accel Calibration Load Method */ - 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_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(accel_offset, mAccelAccuracy); - inv_get_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 Accel Calibration Load Method */ - - inv_set_device_properties(); - - /* disable driver master enable the first sensor goes on */ - masterEnable(0); - enableGyro(0); - enableAccel(0); - enableCompass(0); - - if (isLowPowerQuatEnabled()) { - enableLPQuaternion(0); - } - - onPower(0); - - if (isDmpDisplayOrientationOn()) { - enableDmpOrientation(!isDmpScreenAutoRotationEnabled()); - } - -} - -void MPLSensor::enable_iio_sysfs() -{ - VFUNC_LOG; - - char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN]; - FILE *tempFp = NULL; - - /* ignore failures */ - write_sysfs_int(mpu.in_timestamp_en, 1); - - LOGV_IF(SYSFS_VERBOSE, - "HAL:sysfs:cat %s (%lld)", - mpu.trigger_name, getTimestamp()); - tempFp = fopen(mpu.trigger_name, "r"); - if (tempFp == NULL) { - LOGE("HAL:could not open trigger name"); - } else { - if (fscanf(tempFp, "%s", iio_trigger_name) < 0) { - LOGE("HAL:could not read trigger name"); - } - fclose(tempFp); - } - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)", - iio_trigger_name, mpu.current_trigger, getTimestamp()); - tempFp = fopen(mpu.current_trigger, "w"); - if (tempFp == NULL) { - LOGE("HAL:could not open current trigger"); - } else { - if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) { - LOGE("HAL:could not write current trigger %s err=%d", iio_trigger_name, errno); - } - } - - write_sysfs_int(mpu.buffer_length, IIO_BUFFER_LENGTH); - - if (inv_get_iio_device_node(iio_device_node) < 0) { - LOGE("HAL:could retrive the 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(PROCESS_VERBOSE, "HAL:iio iio_fd (%s) opened: %d", iio_device_node, iio_fd); - } -} - -int MPLSensor::inv_constructor_init() -{ - 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() -{ - VFUNC_LOG; - - inv_error_t result; - - 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_motion_no_motion(); - 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(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled"); - result = inv_enable_vector_compass_cal(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } else { - mFeatureActiveMask |= INV_COMPASS_CAL; - } - - // specify MPL's trust weight, used by compass algorithms - inv_vector_compass_cal_sensitivity(3); - - 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; - } - } - - result = inv_enable_9x_sensor_fusion(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } else { - // 9x sensor fusion enables Compass fit - mFeatureActiveMask |= INV_COMPASS_FIT; - } - - result = inv_enable_no_gyro_fusion(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - result = inv_enable_quat_accuracy_monitor(); - if (result) { - LOG_RESULT_LOCATION(result); - return result; - } - - return result; -} - -/* TODO: create function pointers to calculate scale */ -void MPLSensor::inv_set_device_properties() -{ - 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, 2000L << 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, 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); -} - -void MPLSensor::loadDMP() -{ - int res, fd; - FILE *fptr; - - if (isMpu3050()) { - //DMP support only for MPU6xxx/9xxx currently - 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"); - return; - } - if(inv_read_dmp_state(fd)) { - LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded"); - return; - } - - LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware); - fptr = fopen(mpu.dmp_firmware, "w"); - if(!fptr) { - LOGE("HAL:could open %s for write. %s", mpu.dmp_firmware, strerror(errno)); - return; - } - res = inv_load_dmp(fptr); - if(res < 0) { - LOGE("HAL:load DMP failed"); - } else { - LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded"); - } - fclose(fptr); -} - -void MPLSensor::inv_get_sensors_orientation() -{ - 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]; - 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]); - fclose(fptr); - - 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]; - } else { - LOGE("HAL:Couldn't read 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]; - 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]); - fclose(fptr); - - 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]; - } else { - LOGE("HAL:Couldn't read accel mounting matrix"); - } -} - -MPLSensor::~MPLSensor() -{ - VFUNC_LOG; - - mCompassSensor = NULL; - - /* 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); - - if (isDmpDisplayOrientationOn()) { - closeDmpOrientFd(); - } - - /* Turn off Gyro master enable */ - /* A workaround until driver handles it */ - /* TODO: Turn off and close all sensors */ - if(write_sysfs_int(mpu.chip_enable, 0) < 0) { - LOGE("HAL:could not disable gyro master enable"); - } - -#ifdef INV_PLAYBACK_DBG - inv_turn_off_data_logging(); - fclose(logfile); -#endif -} - -#define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors) -#define A_ENABLED ((1 << ID_A) & enabled_sensors) -#define M_ENABLED ((1 << ID_M) & 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) - -/* TODO: this step is optional, remove? */ -int MPLSensor::setGyroInitialState() -{ - VFUNC_LOG; - - int res = 0; - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", - HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp()); - int fd = open(mpu.gyro_fifo_rate, O_RDWR); - res = errno; - if(fd < 0) { - LOGE("HAL:open of %s failed with '%s' (%d)", - mpu.gyro_fifo_rate, strerror(res), res); - return res; - } - res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ); - if(res < 0) { - LOGE("HAL:write_attribute_sensor : error writing %s with %d", - mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ); - return res; - } - - // Setting LPF is deprecated - - return 0; -} - -/* 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; - } - return 0; -} - -int MPLSensor::onPower(int en) -{ - VFUNC_LOG; - - int res; - - int curr_power_state; - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", - en, mpu.power_state, getTimestamp()); - res = read_sysfs_int(mpu.power_state, &curr_power_state); - if (res < 0) { - LOGE("HAL:Error reading power state"); - // will set power_state anyway - curr_power_state = -1; - } - if (en != curr_power_state) { - if((res = write_sysfs_int(mpu.power_state, en)) < 0) { - LOGE("HAL:Couldn't write power state"); - } - } else { - LOGV_IF(EXTRA_VERBOSE, - "HAL:Power state already enable/disable curr=%d new=%d", - curr_power_state, en); - } - return res; -} - -int MPLSensor::onDMP(int en) -{ - VFUNC_LOG; - - int res = -1; - int status; - - //Sequence to enable DMP - //1. Turn On power if not already on - //2. Load DMP image if not already loaded - //3. Either Gyro or Accel must be enabled/configured before next step - //4. Enable DMP - - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", - mpu.firmware_loaded, getTimestamp()); - res = read_sysfs_int(mpu.firmware_loaded, &status); - if (res < 0){ - LOGE("HAL:ERR can't get firmware_loaded status"); - return res; - } - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: %s status=%d", mpu.firmware_loaded, status); - - if (status) { - //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) { - res = write_sysfs_int(mpu.dmp_on, en); - //Enable DMP interrupt - if (write_sysfs_int(mpu.dmp_int_on, en) < 0) { - LOGE("HAL:ERR can't en/dis DMP interrupt"); - } - } else { - res = 0; //DMP already set as requested - } - } else { - LOGE("HAL:ERR No DMP image"); - } - return res; -} - -int MPLSensor::checkLPQuaternion(void) -{ - VFUNC_LOG; - - return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0); -} - -int MPLSensor::enableLPQuaternion(int en) -{ - VFUNC_LOG; - - if (!en) { - enableQuaternionData(0); - onDMP(0); - mFeatureActiveMask &= ~INV_DMP_QUATERNION; - LOGV_IF(PROCESS_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(PROCESS_VERBOSE, "HAL:LP Quat enabled"); - } - } - return 0; -} - -int MPLSensor::enableQuaternionData(int en) -{ - int res = 0; - VFUNC_LOG; - - // Enable DMP quaternion - res = write_sysfs_int(mpu.quaternion_on, en); - - if (!en) { - LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems"); - } else { - - LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems"); - } - write_sysfs_int(mpu.in_quat_r_en, en); - write_sysfs_int(mpu.in_quat_x_en, en); - write_sysfs_int(mpu.in_quat_y_en, en); - write_sysfs_int(mpu.in_quat_z_en, en); - - LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off"); - - if (!en) { - inv_quaternion_sensor_was_turned_off(); - } - - return res; -} - -int MPLSensor::enableTap(int /*en*/) -{ - VFUNC_LOG; - - return 0; -} - -int MPLSensor::enableFlick(int /*en*/) -{ - VFUNC_LOG; - - return 0; -} - -int MPLSensor::enablePedometer(int /*en*/) -{ - VFUNC_LOG; - - return 0; -} - -int MPLSensor::masterEnable(int en) -{ - VFUNC_LOG; - return write_sysfs_int(mpu.chip_enable, en); -} - -int MPLSensor::enableGyro(int en) -{ - VFUNC_LOG; - - /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */ - int res; - - /* need to also turn on/off the master enable */ - res = write_sysfs_int(mpu.gyro_enable, en); - - if (!en) { - LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off"); - inv_gyro_was_turned_off(); - } else { - write_sysfs_int(mpu.gyro_x_fifo_enable, en); - write_sysfs_int(mpu.gyro_y_fifo_enable, en); - res = write_sysfs_int(mpu.gyro_z_fifo_enable, en); - } - - return res; -} - -int MPLSensor::enableAccel(int en) -{ - VFUNC_LOG; - - /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */ - int res; - - /* need to also turn on/off the master enable */ - res = write_sysfs_int(mpu.accel_enable, en); - - if (!en) { - LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off"); - inv_accel_was_turned_off(); - } else { - write_sysfs_int(mpu.accel_x_fifo_enable, en); - write_sysfs_int(mpu.accel_y_fifo_enable, en); - res = write_sysfs_int(mpu.accel_z_fifo_enable, en); - } - - return res; -} - -int MPLSensor::enableCompass(int en) -{ - VFUNC_LOG; - - int res = mCompassSensor->enable(ID_M, en); - if (!en) { - LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off"); - inv_compass_was_turned_off(); - } - return res; -} - -void MPLSensor::computeLocalSensorMask(int enabled_sensors) -{ - VFUNC_LOG; - - do { - if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { - LOGV_IF(ENG_VERBOSE, "FUSION ENABLED"); - mLocalSensorMask = ALL_MPL_SENSORS_NP; - break; - } - - if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) { - /* Invensense compass cal */ - LOGV_IF(ENG_VERBOSE, "ALL DISABLED"); - mLocalSensorMask = 0; - break; - } - - if (GY_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) { - 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::enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int)) { - LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - %s %s", en ? "enabled" : "disable", name); - return (this->*enabler)(en); -} - -int MPLSensor::enableSensors(unsigned long sensors, int /*en*/, uint32_t changed) { - VFUNC_LOG; - - inv_error_t res = -1; - bool store_cal = false; - bool ext_compass_changed = false; - - // Sequence to enable or disable a sensor - // 1. enable Power state - // 2. reset master enable (=0) - // 3. enable or disable a sensor - // 4. set master enable (=1) - - pthread_mutex_lock(&GlobalHalMutex); - - uint32_t all_changeables = (1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) - | (1 << MagneticField); - uint32_t all_integrated_changeables = all_changeables; - - if (!mCompassSensor->isIntegrated()) { - ext_compass_changed = changed & (1 << MagneticField); - all_integrated_changeables = all_changeables & ~(1 << MagneticField); - } - - if (isLowPowerQuatEnabled() || (changed & all_integrated_changeables)) { - /* ensure power state is on */ - onPower(1); - - /* reset master enable */ - res = masterEnable(0); - if(res < 0) { - goto unlock_res; - } - } - - LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors); - - if (changed & ((1 << Gyro) | (1 << RawGyro))) { - res = enableOneSensor(sensors & INV_THREE_AXIS_GYRO, "gyro", &MPLSensor::enableGyro); - if(res < 0) { - goto unlock_res; - } - } - - if (changed & (1 << Accelerometer)) { - res = enableOneSensor(sensors & INV_THREE_AXIS_ACCEL, "accel", &MPLSensor::enableAccel); - if(res < 0) { - goto unlock_res; - } - } - - if (changed & (1 << MagneticField)) { - /* Invensense compass calibration */ - res = enableOneSensor(sensors & INV_THREE_AXIS_COMPASS, "compass", &MPLSensor::enableCompass); - if(res < 0) { - goto unlock_res; - } - } - - if ( isLowPowerQuatEnabled() ) { - // Enable LP Quat - if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) | - (1 << LinearAccel) | (1 << Gravity)))) { - if (!(changed & all_integrated_changeables)) { - /* ensure power state is on */ - onPower(1); - /* reset master enable */ - res = masterEnable(0); - if(res < 0) { - goto unlock_res; - } - } - if (!checkLPQuaternion()) { - enableLPQuaternion(1); - } else { - LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled"); - } - } else if (checkLPQuaternion()) { - enableLPQuaternion(0); - } - } - - if (changed & all_integrated_changeables) { - if (sensors & - (INV_THREE_AXIS_GYRO - | INV_THREE_AXIS_ACCEL - | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { - if ( isLowPowerQuatEnabled() || - (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) ) { - // disable DMP event interrupt only (w/ data interrupt) - if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) { - res = -1; - LOGE("HAL:ERR can't disable DMP event interrupt"); - goto unlock_res; - } - } - - if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { - // enable DMP - onDMP(1); - - res = enableAccel(1); - if(res < 0) { - goto unlock_res; - } - - if ((sensors & INV_THREE_AXIS_ACCEL) == 0) { - res = turnOffAccelFifo(); - } - if(res < 0) { - goto unlock_res; - } - } - - res = masterEnable(1); - if(res < 0) { - goto unlock_res; - } - } else { // all sensors idle -> reduce power - if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { - // enable DMP - onDMP(1); - // enable DMP event interrupt only (no data interrupt) - if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) { - res = -1; - LOGE("HAL:ERR can't enable DMP event interrupt"); - } - res = enableAccel(1); - if(res < 0) { - goto unlock_res; - } - if ((sensors & INV_THREE_AXIS_ACCEL) == 0) { - res = turnOffAccelFifo(); - } - if(res < 0) { - goto unlock_res; - } - res = masterEnable(1); - if(res < 0) { - goto unlock_res; - } - } - else { - res = onPower(0); - if(res < 0) { - goto unlock_res; - } - } - store_cal = true; - } - } else if (ext_compass_changed && - !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL - | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) { - store_cal = true; - } - - if (store_cal || ((changed & all_changeables) != all_changeables)) { - storeCalibration(); - } - -unlock_res: - pthread_mutex_unlock(&GlobalHalMutex); - return res; -} - -/* Store calibration file */ -void MPLSensor::storeCalibration() -{ - if(mHaveGoodMpuCal || 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"); - } - } -} - -void MPLSensor::cbProcData() -{ - mNewData = 1; - mSampleCount++; - LOGV_IF(EXTRA_VERBOSE, "HAL:new data"); -} - -/* these handlers transform mpl data into one of the Android sensor types */ -int MPLSensor::gyroHandler(sensors_event_t* s) -{ - VHANDLER_LOG; - int8_t status; - int update; - update = inv_get_sensor_type_gyroscope(s->gyro.v, &status, &s->timestamp); - 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; - int8_t status; - int update; - update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &status, &s->timestamp); - LOGV_IF(HANDLER_DATA, "HAL:raw 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::accelHandler(sensors_event_t* s) -{ - VHANDLER_LOG; - int8_t status; - int update; - update = inv_get_sensor_type_accelerometer( - s->acceleration.v, &status, &s->timestamp); - 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 = status; - return update; -} - -int MPLSensor::compassHandler(sensors_event_t* s) -{ - VHANDLER_LOG; - int update; - update = inv_get_sensor_type_magnetic_field( - s->magnetic.v, &s->magnetic.status, &s->timestamp); - 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; -} - -int MPLSensor::rvHandler(sensors_event_t* s) -{ - // rotation vector does not have an accuracy or status - VHANDLER_LOG; - int8_t status; - int update; - update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp); - LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d", - s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update); - return update; -} - -int MPLSensor::laHandler(sensors_event_t* s) -{ - VHANDLER_LOG; - int8_t status; - int update; - update = inv_get_sensor_type_linear_acceleration( - s->gyro.v, &status, &s->timestamp); - 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; - int8_t status; - int update; - update = inv_get_sensor_type_gravity(s->gyro.v, &status, &s->timestamp); - 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; - update = inv_get_sensor_type_orientation( - s->orientation.v, &s->orientation.status, &s->timestamp); - 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::enable(int32_t handle, int en) -{ - VFUNC_LOG; - - android::String8 sname; - int what = -1, err = 0; - - switch (handle) { - 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()); - /* TODO: stop manually testing/using 0 and 1 instead of - * false and true, but just use 0 and non-0. - * This allows passing 0 and non-0 ints around instead of - * having to convert to 1 and test against 1. - */ - mDmpOrientationEnabled = en; - return 0; - case ID_A: - what = Accelerometer; - sname = "Accelerometer"; - break; - case ID_M: - what = MagneticField; - sname = "MagneticField"; - break; - case ID_O: - what = Orientation; - sname = "Orientation"; - break; - case ID_GY: - what = Gyro; - sname = "Gyro"; - break; - case ID_RG: - what = RawGyro; - sname = "RawGyro"; - break; - case ID_GR: - what = Gravity; - sname = "Gravity"; - break; - case ID_RV: - what = RotationVector; - sname = "RotationVector"; - break; - case ID_LA: - what = LinearAccel; - sname = "LinearAccel"; - break; - default: //this takes care of all the gestures - what = handle; - sname = "Others"; - break; - } - - if (uint32_t(what) >= numSensors) - return -EINVAL; - - 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(PROCESS_VERBOSE, - "HAL:%s sensor state change what=%d", sname.string(), what); - - if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { - short flags = newState; - uint32_t lastEnabled = mEnabled, changed = 0; - - mEnabled &= ~(1 << what); - mEnabled |= (uint32_t(flags) << what); - - LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle); - LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags); - computeLocalSensorMask(mEnabled); - LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled); - sen_mask = mLocalSensorMask & mMasterSensorMask; - mSensorMask = sen_mask; - LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask); - - switch (what) { - case Gyro: - case RawGyro: - case Accelerometer: - case MagneticField: - if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) | - (1 << LinearAccel) | (1 << Gravity))) && - ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) { - changed |= (1 << what); - } - break; - - case Orientation: - case RotationVector: - case LinearAccel: - case Gravity: - if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) | - (1 << LinearAccel) | (1 << Gravity)))) || - (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) | - (1 << LinearAccel) | (1 << Gravity))))) { - for (int i = Gyro; i <= MagneticField; i++) { - if (!(mEnabled & (1 << i))) { - changed |= (1 << i); - } - } - } - break; - } - LOGV_IF(PROCESS_VERBOSE, "HAL:changed = %d", changed); - enableSensors(sen_mask, flags, changed); - } - -#ifdef INV_PLAYBACK_DBG - /* apparently the logging needs to be go through this sequence - to properly flush the log file */ - inv_turn_off_data_logging(); - fclose(logfile); - logfile = fopen("/data/playback.bin", "ab"); - if (logfile) - inv_turn_on_data_logging(logfile); -#endif - - return err; -} - -int MPLSensor::setDelay(int32_t handle, int64_t ns) -{ - VFUNC_LOG; - - android::String8 sname; - int what = -1; - - switch (handle) { - case ID_SO: - return update_delay(); - case ID_A: - what = Accelerometer; - sname = "Accelerometer"; - break; - case ID_M: - what = MagneticField; - sname = "MagneticField"; - break; - case ID_O: - what = Orientation; - sname = "Orientation"; - break; - case ID_GY: - what = Gyro; - sname = "Gyro"; - break; - case ID_RG: - what = RawGyro; - sname = "RawGyro"; - break; - case ID_GR: - what = Gravity; - sname = "Gravity"; - break; - case ID_RV: - what = RotationVector; - sname = "RotationVector"; - break; - case ID_LA: - what = LinearAccel; - sname = "LinearAccel"; - break; - default: // this takes care of all the gestures - what = handle; - sname = "Others"; - break; - } - -#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 - - 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 array for each sensor */ - mDelays[what] = ns; - - switch (what) { - case Gyro: - case RawGyro: - case Accelerometer: - for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated(); - i++) { - if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { - LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i); - return 0; - } - } - break; - - case MagneticField: - if (mCompassSensor->isIntegrated() && - (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) || - ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) || - ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) { - LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel"); - return 0; - } - break; - - case Orientation: - case RotationVector: - case LinearAccel: - case Gravity: - if (isLowPowerQuatEnabled()) { - LOGV_IF(PROCESS_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(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i); - return 0; - } - } - break; - } - - int res = update_delay(); - return res; -} - -int MPLSensor::update_delay() { - VHANDLER_LOG; - - int res = 0; - int64_t got; - - pthread_mutex_lock(&GlobalHalMutex); - if (mEnabled) { - int64_t wanted = 1000000000; - int64_t wanted_3rd_party_sensor = 1000000000; - - // Sequence to change sensor's FIFO rate - // 1. enable Power state - // 2. reset master enable - // 3. Update delay - // 4. set master enable - - // ensure power on - onPower(1); - - // reset master enable - masterEnable(0); - - /* 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; - } - } - - // same delay for 3rd party Accel or Compass - wanted_3rd_party_sensor = wanted; - - /* mpl rate in us in future maybe different for - gyro vs compass vs accel */ - int rateInus = (int)wanted / 1000LL; - int mplGyroRate = rateInus; - int mplAccelRate = rateInus; - int mplCompassRate = rateInus; - - LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : " - "%llu ns, mpl rate: %d us, (%.2f Hz)", - wanted, rateInus, 1000000000.f / wanted); - - /* 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); -#ifdef LIBMLLITE_FROM_SOURCE - inv_set_linear_acceleration_sample_rate(rateInus); - inv_set_gravity_sample_rate(rateInus); -#endif - - /* TODO: Test 200Hz */ - // inv_set_gyro_sample_rate(5000); - LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate); - LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate); - LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate); - - int enabled_sensors = mEnabled; - int tempFd = -1; - if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { - if (isLowPowerQuatEnabled() || - (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) { - bool setDMPrate= 0; - // Set LP Quaternion sample rate if enabled - if (checkLPQuaternion()) { - if (wanted < RATE_200HZ) { - enableLPQuaternion(0); - } else { - inv_set_quat_sample_rate(rateInus); - setDMPrate= 1; - } - } - - if (checkDMPOrientation() || setDMPrate==1) { - getDmpRate(&wanted); - } - } - - int64_t tempRate = wanted; - LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion"); - //nsToHz - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", - 1000000000.f / tempRate, mpu.gyro_fifo_rate, - getTimestamp()); - tempFd = open(mpu.gyro_fifo_rate, O_RDWR); - res = write_attribute_sensor(tempFd, 1000000000.f / tempRate); - if(res < 0) { - LOGE("HAL:GYRO update delay error"); - } - - //nsToHz (BMA250) - if(USE_THIRD_PARTY_ACCEL) { - LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)", - wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate, - getTimestamp()); - tempFd = open(mpu.accel_fifo_rate, O_RDWR); - res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L); - LOGE_IF(res < 0, "HAL:ACCEL update delay error"); - } - - if (!mCompassSensor->isIntegrated()) { - LOGV_IF(PROCESS_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 { - - if (GY_ENABLED) { - wanted = (mDelays[Gyro] <= mDelays[RawGyro]? - (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]): - (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro])); - - if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { - 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:GYRO update delay error"); - } - - if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */ - if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) { - wanted = mDelays[Gyro]; - } - else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) { - wanted = mDelays[RawGyro]; - - } else { - wanted = mDelays[Accelerometer]; - } - - if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { - getDmpRate(&wanted); - } - - /* TODO: use function pointers to calculate delay value specific to vendor */ - LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", - 1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp()); - tempFd = open(mpu.accel_fifo_rate, O_RDWR); - if(USE_THIRD_PARTY_ACCEL) { - //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) { - if (!mCompassSensor->isIntegrated()) { - wanted = mDelays[MagneticField]; - } else { - if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) { - wanted = mDelays[Gyro]; - } - else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) { - wanted = mDelays[RawGyro]; - } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) { - wanted = mDelays[Accelerometer]; - } else { - wanted = mDelays[MagneticField]; - } - - if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { - getDmpRate(&wanted); - } - } - - mCompassSensor->setDelay(ID_M, wanted); - got = mCompassSensor->getDelay(ID_M); - inv_set_compass_sample_rate(got / 1000); - } - - } - - unsigned long sensors = mLocalSensorMask & mMasterSensorMask; - if (sensors & - (INV_THREE_AXIS_GYRO - | INV_THREE_AXIS_ACCEL - | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { - res = masterEnable(1); - } else { // all sensors idle -> reduce power - res = onPower(0); - } - } - pthread_mutex_unlock(&GlobalHalMutex); - return res; -} - -/* For Third Party Accel Input Subsystem Drivers only */ -/* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */ -int MPLSensor::readAccelEvents(sensors_event_t* /*data*/, int count) -{ - VHANDLER_LOG; - - if (count < 1) - return -EINVAL; - - ssize_t n = mAccelInputReader.fill(accel_fd); - if (n < 0) { - LOGE("HAL:missed accel events, exit"); - return n; - } - - int numEventReceived = 0; - input_event const* event; - int done = 0; - - while (!done && count && mAccelInputReader.readEvent(&event)) { - int type = event->type; - if (type == EV_ABS) { - if (event->code == EVENT_TYPE_ACCEL_X) { - mPendingMask |= 1 << Accelerometer; - mCachedAccelData[0] = event->value; - } else if (event->code == EVENT_TYPE_ACCEL_Y) { - mPendingMask |= 1 << Accelerometer; - mCachedAccelData[1] = event->value; - } else if (event->code == EVENT_TYPE_ACCEL_Z) { - mPendingMask |= 1 << Accelerometer; - mCachedAccelData[2] =event-> value; - } - } else if (type == EV_SYN) { - done = 1; - if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { - inv_build_accel(mCachedAccelData, 0, getTimestamp()); - } - } else { - LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)", - type, event->code); - } - mAccelInputReader.next(); - } - - return numEventReceived; -} - -/** - * 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. - */ -/* TODO: This should probably be called "int readEvents(...)" - * and readEvents() called "void cacheData(void)". - */ -int MPLSensor::executeOnData(sensors_event_t* data, int count) -{ - VFUNC_LOG; - - inv_execute_on_data(); - - int numEventReceived = 0; - - long msg; - 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; - } - } - - // load up virtual sensors - for (int i = 0; i < numSensors; i++) { - int update; - if (mEnabled & (1 << i)) { - update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i); - mPendingMask |= (1 << i); - - if (update && (count > 0)) { - *data++ = mPendingEvents[i]; - count--; - numEventReceived++; - } - } - } - - return numEventReceived; -} - -// collect data for MPL (but NOT sensor service currently), from driver layer -/* TODO: FIX! data and count are not used, results is hardcoded to 0 */ -/* TODO: This should probably be called "void cacheEvents(void)" - * And executeOnData() should be int readEvents(data,count) - */ -int MPLSensor::readEvents(sensors_event_t* /*data*/, int /*count*/) { - - - int lp_quaternion_on = 0, nbyte; - int i, mask = 0, numEventReceived = 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); - char *rdata = mIIOBuffer; - - nbyte= (8 * sensors + 8) * 1; - - if (isLowPowerQuatEnabled()) { - lp_quaternion_on = checkLPQuaternion(); - if (lp_quaternion_on) { - nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data - } - } - - ssize_t rsize = read(iio_fd, rdata, nbyte); - if (sensors == 0) { - rsize = read(iio_fd, rdata, sizeof(mIIOBuffer)); - } - -#ifdef TESTING - LOGI("get one sample of IIO data with size: %d", rsize); - LOGI("sensors: %d", sensors); - - LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d", - *((short *) (rdata + 0)), *((short *) (rdata + 2)), - *((short *) (rdata + 4))); - LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d", - *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))), - *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))), - *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))); - - LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS & - mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d", - *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + - ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))), - *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + - ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))), - *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + - ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))); -#endif - - if (rsize != nbyte) { - LOGE("HAL:ERR Full data packet was not read. rsize=%zd nbyte=%d sensors=%d errno=%d(%s)", - rsize, nbyte, sensors, errno, strerror(errno)); - rsize = read(iio_fd, rdata, sizeof(mIIOBuffer)); - return -1; - } - - if (isLowPowerQuatEnabled() && lp_quaternion_on) { - - for (i=0; i< 4; i++) { - mCachedQuaternionData[i]= *(long*)rdata; - rdata += sizeof(long); - } - } - - for (i = 0; i < 3; i++) { - if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { - mCachedGyroData[i] = *((short *) (rdata + i * 2)); - } - if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { - mCachedAccelData[i] = *((short *) (rdata + i * 2 + - ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))); - } - if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) { - mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1))); - } - } - - mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) + - ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0)); - if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() && - (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) { - mask |= 1 << MagneticField; - } - - mSensorTimestamp = *((long long *) (rdata + 8 * sensors)); - if (mCompassSensor->isIntegrated()) { - mCompassTimestamp = mSensorTimestamp; - } - - if (mask & (1 << Gyro)) { - // send down temperature every 0.5 seconds - // with timestamp measured in "driver" layer - if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) { - mTempCurrentTime = mSensorTimestamp; - long long temperature[2]; - if(inv_read_temperature(temperature) == 0) { - LOGV_IF(INPUT_DATA, - "HAL:inv_read_temperature = %lld, timestamp= %lld", - temperature[0], temperature[1]); - inv_build_temp(temperature[0], temperature[1]); - } -#ifdef TESTING - long bias[3], temp, temp_slope[3]; - inv_get_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; - - if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { - inv_build_gyro(mCachedGyroData, mSensorTimestamp); - LOGV_IF(INPUT_DATA, - "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld", - mCachedGyroData[0], mCachedGyroData[1], - mCachedGyroData[2], mSensorTimestamp); - } - } - - if (mask & (1 << Accelerometer)) { - mPendingMask |= 1 << Accelerometer; - if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { - inv_build_accel(mCachedAccelData, 0, mSensorTimestamp); - LOGV_IF(INPUT_DATA, - "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld", - mCachedAccelData[0], mCachedAccelData[1], - mCachedAccelData[2], mSensorTimestamp); - } - } - - if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) { - int status = 0; - if (mCompassSensor->providesCalibration()) { - status = mCompassSensor->getAccuracy(); - status |= INV_CALIBRATED; - } - if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) { - inv_build_compass(mCachedCompassData, status, - mCompassTimestamp); - LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", - mCachedCompassData[0], mCachedCompassData[1], - mCachedCompassData[2], mCompassTimestamp); - } - } - - if (isLowPowerQuatEnabled() && lp_quaternion_on) { - - inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp); - LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld", - mCachedQuaternionData[0], mCachedQuaternionData[1], - mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp); - } - - return numEventReceived; -} - -/* use for both MPUxxxx and third party compass */ -int MPLSensor::readCompassEvents(sensors_event_t* /*data*/, int count) -{ - VHANDLER_LOG; - - if (count < 1) - return -EINVAL; - - int numEventReceived = 0; - int done = 0; - - done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp); -#ifdef COMPASS_YAS53x - if (mCompassSensor->checkCoilsReset()) { - //Reset relevant compass settings - resetCompass(); - } -#endif - if (done > 0) { - int status = 0; - if (mCompassSensor->providesCalibration()) { - status = mCompassSensor->getAccuracy(); - status |= INV_CALIBRATED; - } - if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) { - inv_build_compass(mCachedCompassData, status, - mCompassTimestamp); - LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", - mCachedCompassData[0], mCachedCompassData[1], - mCachedCompassData[2], mCompassTimestamp); - } - } - - return numEventReceived; -} - -#ifdef COMPASS_YAS53x -int MPLSensor::resetCompass() -{ - VFUNC_LOG; - - //Reset compass cal if enabled - if (mFeatureActiveMask & INV_COMPASS_CAL) { - LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal"); - inv_init_vector_compass_cal(); - } - - //Reset compass fit if enabled - if (mFeatureActiveMask & INV_COMPASS_FIT) { - LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit"); - inv_init_compass_fit(); - } - - return 0; -} -#endif - -int MPLSensor::getFd() const -{ - VFUNC_LOG; - LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd); - return iio_fd; -} - -int MPLSensor::getAccelFd() const -{ - VFUNC_LOG; - LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd); - return accel_fd; -} - -int MPLSensor::getCompassFd() const -{ - VFUNC_LOG; - int fd = mCompassSensor->getFd(); - LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd); - return fd; -} - -int MPLSensor::turnOffAccelFifo() { - int i, res; - char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable, - mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable}; - - for (i = 0; i < 3; i++) { - res = write_sysfs_int(accel_fifo_enable[i], 0); - if (res < 0) { - return res; - } - } - return 0; -} - -int MPLSensor::enableDmpOrientation(int en) -{ - VFUNC_LOG; - /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */ - int res = 0; - int enabled_sensors = mEnabled; - - if (isMpu3050()) - return res; - - pthread_mutex_lock(&GlobalHalMutex); - - // on power if not already On - res = onPower(1); - // reset master enable - res = masterEnable(0); - - if (en) { - //Enable DMP orientation - if (write_sysfs_int(mpu.display_orientation_on, en) < 0) { - LOGE("HAL:ERR can't enable Android orientation"); - res = -1; // indicate an err - } - - // open DMP Orient Fd - res = openDmpOrientFd(); - - // enable DMP - res = onDMP(1); - - // default DMP output rate to FIFO - if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) { - LOGE("HAL:ERR can't default DMP output rate"); - } - - // set DMP rate to 200Hz - if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) { - res = -1; - LOGE("HAL:ERR can't set DMP rate to 200Hz"); - } - - // enable accel engine - res = enableAccel(1); - - // disable accel FIFO - if (!A_ENABLED) { - res = turnOffAccelFifo(); - } - - mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION; - } else { - // disable DMP - res = onDMP(0); - - // disable accel engine - if (!A_ENABLED) { - res = enableAccel(0); - } - } - - res = masterEnable(1); - pthread_mutex_unlock(&GlobalHalMutex); - return res; -} - -int MPLSensor::openDmpOrientFd() -{ - VFUNC_LOG; - - if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) { - LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened"); - return -1; - } - - 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() -{ - 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; - } - fscanf(fp, "%d\n", &screen_orientation); - fclose(fp); - - int numEventReceived = 0; - - if (mDmpOrientationEnabled && count > 0) { - sensors_event_t temp; - - bzero(&temp, sizeof(temp)); /* Let's hope that SENSOR_TYPE_NONE is 0 */ - temp.version = sizeof(sensors_event_t); - temp.sensor = ID_SO; -#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION - temp.type = SENSOR_TYPE_SCREEN_ORIENTATION; - temp.screen_orientation = screen_orientation; -#endif - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; - - *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() -{ - VFUNC_LOG; - - LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd); - return dmp_orient_fd; - -} - -int MPLSensor::checkDMPOrientation() -{ - VFUNC_LOG; - return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0); -} - -int MPLSensor::getDmpRate(int64_t *wanted) -{ - if (checkDMPOrientation() || checkLPQuaternion()) { - // set DMP output rate to FIFO - write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted); - LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *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() -{ - VHANDLER_LOG; - return mPollTime; -} - -bool MPLSensor::hasPendingEvents() const -{ - VHANDLER_LOG; - // if we are using the polling workaround, force the main - // loop to check for data every time - return (mPollTime != -1); -} - -/* TODO: support resume suspend when we gain more info about them*/ -void MPLSensor::sleepEvent() -{ - VFUNC_LOG; -} - -void MPLSensor::wakeEvent() -{ - VFUNC_LOG; -} - -int MPLSensor::inv_float_to_q16(float *fdata, long *ldata) -{ - VHANDLER_LOG; - - if (!fdata || !ldata) - return -1; - ldata[0] = (long)(fdata[0] * 65536.f); - ldata[1] = (long)(fdata[1] * 65536.f); - ldata[2] = (long)(fdata[2] * 65536.f); - return 0; -} - -int MPLSensor::inv_long_to_q16(long *fdata, long *ldata) -{ - VHANDLER_LOG; - - if (!fdata || !ldata) - return -1; - ldata[0] = (fdata[1] * 65536.f); - ldata[1] = (fdata[2] * 65536.f); - ldata[2] = (fdata[3] * 65536.f); - return 0; -} - -int MPLSensor::inv_float_to_round(float *fdata, long *ldata) -{ - VHANDLER_LOG; - - if (!fdata || !ldata) - return -1; - ldata[0] = (long)fdata[0]; - ldata[1] = (long)fdata[1]; - ldata[2] = (long)fdata[2]; - return 0; -} - -int MPLSensor::inv_float_to_round2(float *fdata, short *ldata) -{ - VHANDLER_LOG; - - if (!fdata || !ldata) - return -1; - ldata[0] = (short)fdata[0]; - ldata[1] = (short)fdata[1]; - ldata[2] = (short)fdata[2]; - return 0; -} - -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, - "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(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) { - LOGE("HAL:sensor list too small, not populating."); - return -(sizeof(sSensorList) / sizeof(sensor_t)); - } - - /* fill in the base values */ - memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t))); - - /* first add gyro, accel and compass to the list */ - - /* fill in gyro/accel values */ - fillGyro(chip_ID, list); - fillAccel(chip_ID, list); - - // TODO: need fixes for unified HAL and 3rd-party solution - mCompassSensor->fillList(&list[MagneticField]); - - if(1) { - numsensors = (sizeof(sSensorList) / 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 gravity values */ - fillGravity(list); - /* fill in Linear accel values */ - fillLinearAccel(list); - } 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, "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, "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 bma250 and might be wrong.", - accel); - list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; - list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; - list[Accelerometer].power = ACCEL_BMA250_POWER; - list[Accelerometer].minDelay = ACCEL_BMA250_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, "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 { - LOGE("HAL:unknown gyro id -- gyro params will be wrong."); - LOGE("HAL:default to use mpu3050 params"); - list[Gyro].maxRange = GYRO_MPU3050_RANGE; - list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; - list[Gyro].power = GYRO_MPU3050_POWER; - list[Gyro].minDelay = GYRO_MPU3050_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 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; -} - -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; -} - -int MPLSensor::inv_init_sysfs_attributes(void) -{ - VFUNC_LOG; - - unsigned char i; - char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN]; - char *sptr; - char **dptr; - - sysfs_names_ptr = - (char*)calloc(1, sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); - sptr = sysfs_names_ptr; - if (sptr == NULL) { - LOGE("HAL:couldn't alloc mem for sysfs paths"); - return -1; - } - - dptr = (char**)&mpu; - i = 0; - do { - *dptr++ = sptr; - sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); - } while (++i < MAX_SYSFS_ATTRB); - - // get proper (in absolute/relative) IIO path & build MPU's sysfs paths - // inv_get_sysfs_abs_path(sysfs_path); - if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) { - ALOGE("MPLSensor failed get sysfs path"); - return -1; - } - - if(INV_SUCCESS != inv_get_iio_trigger_path(iio_trigger_path)) { - ALOGE("MPLSensor failed get iio trigger path"); - return -1; - } - - 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.power_state, "%s%s", sysfs_path, "/power_state"); - sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en"); - sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name"); - sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger"); - - 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.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate"); - sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on"); - - // TODO: for self test - 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_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en"); - sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en"); - sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en"); - - sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable"); - sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); - sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix"); - - -#ifndef THIRD_PARTY_ACCEL //MPUxxxx - sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale"); - // TODO: for bias settings - sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias"); -#endif - - sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en"); - sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en"); - sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en"); - - sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on"); - sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en"); - sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en"); - sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en"); - sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en"); - - sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on"); - sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation"); - -#if SYSFS_VERBOSE - // test print sysfs paths - dptr = (char**)&mpu; - for (i = 0; i < MAX_SYSFS_ATTRB; i++) { - LOGE("HAL:sysfs path: %s", *dptr++); - } -#endif - return 0; -} - -/* TODO: stop manually testing/using 0 and 1 instead of - * false and true, but just use 0 and non-0. - * This allows passing 0 and non-0 ints around instead of - * having to convert to 1 and test against 1. - */ -bool MPLSensor::isMpu3050() -{ - return !strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050"); -} - -int MPLSensor::isLowPowerQuatEnabled() -{ -#ifdef ENABLE_LP_QUAT_FEAT - return !isMpu3050(); -#else - return 0; -#endif -} - -int MPLSensor::isDmpDisplayOrientationOn() -{ -#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT - return !isMpu3050(); -#else - return 0; -#endif -} |