MPLSensor.cpp revision 42331858975144405f95243be8427084ee7d478d
142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* 242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * Copyright (C) 2011 Invensense, Inc. 342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * 442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * Licensed under the Apache License, Version 2.0 (the "License"); 542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * you may not use this file except in compliance with the License. 642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * You may obtain a copy of the License at 742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * 842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * http://www.apache.org/licenses/LICENSE-2.0 942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * 1042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * Unless required by applicable law or agreed to in writing, software 1142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * distributed under the License is distributed on an "AS IS" BASIS, 1242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * See the License for the specific language governing permissions and 1442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * limitations under the License. 1542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 1642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru//#define LOG_NDEBUG 0 1842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru//see also the EXTRA_VERBOSE define, below 1942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 2042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <fcntl.h> 2142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <errno.h> 2242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <math.h> 2342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <float.h> 2442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <poll.h> 2542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <unistd.h> 2642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <dirent.h> 2742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <stdlib.h> 2842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <sys/select.h> 2942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <dlfcn.h> 3042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <pthread.h> 3142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 3242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <cutils/log.h> 3342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <utils/KeyedVector.h> 3442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 3542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "MPLSensor.h" 3642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 3742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "math.h" 3842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "ml.h" 3942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mlFIFO.h" 4042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mlsl.h" 4142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mlos.h" 4242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "ml_mputest.h" 4342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "ml_stored_data.h" 4442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mldl_cfg.h" 4542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mldl.h" 4642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 4742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mpu.h" 4842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "kernel/timerirq.h" 4942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "kernel/mpuirq.h" 5042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "kernel/slaveirq.h" 5142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 5242331858975144405f95243be8427084ee7d478dJean-Baptiste Queruextern "C" { 5342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mlsupervisor.h" 5442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 5542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 5642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mlcontrol.h" 5742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 5842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define EXTRA_VERBOSE (0) 5942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__) 6042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define VFUNC_LOG LOGV_IF(EXTRA_VERBOSE, "%s", __PRETTY_FUNCTION__) 6142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* this mask must turn on only the sensors that are present and managed by the MPL */ 6242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_COMPASS | INV_THREE_AXIS_GYRO) 6342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 6442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define CALL_MEMBER_FN(pobject,ptrToMember) ((pobject)->*(ptrToMember)) 6542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 6642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* *************************************************************************** 6742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * MPL interface misc. 6842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 6942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru//static pointer to the object that will handle callbacks 7042331858975144405f95243be8427084ee7d478dJean-Baptiste Querustatic MPLSensor* gMPLSensor = NULL; 7142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 7242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* we need to pass some callbacks to the MPL. The mpl is a C library, so 7342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * wrappers for the C++ callback implementations are required. 7442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 7542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruextern "C" { 7642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru//callback wrappers go here 7742331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid mot_cb_wrapper(uint16_t val) 7842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 7942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (gMPLSensor) { 8042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru gMPLSensor->cbOnMotion(val); 8142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 8242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 8342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 8442331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid procData_cb_wrapper() 8542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 8642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if(gMPLSensor) { 8742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru gMPLSensor->cbProcData(); 8842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 8942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 9042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 9142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} //end of extern C 9242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 9342331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid setCallbackObject(MPLSensor* gbpt) 9442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 9542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru gMPLSensor = gbpt; 9642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 9742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 9842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 9942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/***************************************************************************** 10042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * sensor class implementation 10142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 10242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 10342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define GY_ENABLED ((1<<ID_GY) & enabled_sensors) 10442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define A_ENABLED ((1<<ID_A) & enabled_sensors) 10542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define O_ENABLED ((1<<ID_O) & enabled_sensors) 10642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define M_ENABLED ((1<<ID_M) & enabled_sensors) 10742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define LA_ENABLED ((1<<ID_LA) & enabled_sensors) 10842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define GR_ENABLED ((1<<ID_GR) & enabled_sensors) 10942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define RV_ENABLED ((1<<ID_RV) & enabled_sensors) 11042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 11142331858975144405f95243be8427084ee7d478dJean-Baptiste QueruMPLSensor::MPLSensor() : 11242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru SensorBase(NULL, NULL), mEnabled(0), mPendingMask(0), mMpuAccuracy(0), 11342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mNewData(0), mDmpStarted(false), 11442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mMplMutex(PTHREAD_MUTEX_INITIALIZER), 11542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mMasterSensorMask(INV_ALL_SENSORS), 11642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1), 11742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mCurFifoRate(-1), mHaveGoodMpuCal(false), 11842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mUseTimerIrqAccel(false), mUsetimerIrqCompass(true), 11942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mUseTimerirq(false), mSampleCount(0) 12042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 12142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 12242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t rv; 12342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int mpu_int_fd, i; 12442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru char *port = NULL; 12542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 12642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors); 12742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 12842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mForceSleep = false; 12942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 13042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (i = 0; i < ARRAY_SIZE(mPollFds); i++) { 13142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[i].fd = -1; 13242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[i].events = 0; 13342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 13442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 13542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 13642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 13742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mpu_int_fd = open("/dev/mpuirq", O_RDWR); 13842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mpu_int_fd == -1) { 13942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("could not open the mpu irq device node"); 14042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 14142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK); 14242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //ioctl(mpu_int_fd, MPUIRQ_SET_TIMEOUT, 0); 14342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mIrqFds.add(MPUIRQ_FD, mpu_int_fd); 14442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[MPUIRQ_FD].fd = mpu_int_fd; 14542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[MPUIRQ_FD].events = POLLIN; 14642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 14742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 14842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru accel_fd = open("/dev/accelirq", O_RDWR); 14942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (accel_fd == -1) { 15042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("could not open the accel irq device node"); 15142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 15242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru fcntl(accel_fd, F_SETFL, O_NONBLOCK); 15342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //ioctl(accel_fd, SLAVEIRQ_SET_TIMEOUT, 0); 15442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mIrqFds.add(ACCELIRQ_FD, accel_fd); 15542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[ACCELIRQ_FD].fd = accel_fd; 15642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[ACCELIRQ_FD].events = POLLIN; 15742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 15842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 15942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru timer_fd = open("/dev/timerirq", O_RDWR); 16042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (timer_fd == -1) { 16142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("could not open the timer irq device node"); 16242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 16342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru fcntl(timer_fd, F_SETFL, O_NONBLOCK); 16442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //ioctl(timer_fd, TIMERIRQ_SET_TIMEOUT, 0); 16542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mIrqFds.add(TIMERIRQ_FD, timer_fd); 16642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[TIMERIRQ_FD].fd = timer_fd; 16742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[TIMERIRQ_FD].events = POLLIN; 16842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 16942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 17042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru data_fd = mpu_int_fd; 17142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 17242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if ((accel_fd == -1) && (timer_fd != -1)) { 17342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //no accel irq and timer available 17442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mUseTimerIrqAccel = true; 17542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGD("MPLSensor falling back to timerirq for accel data"); 17642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 17742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 17842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru memset(mPendingEvents, 0, sizeof(mPendingEvents)); 17942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 18042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[RotationVector].version = sizeof(sensors_event_t); 18142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[RotationVector].sensor = ID_RV; 18242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; 18342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[RotationVector].acceleration.status 18442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 18542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 18642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); 18742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].sensor = ID_LA; 18842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; 18942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].acceleration.status 19042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 19142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 19242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].version = sizeof(sensors_event_t); 19342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].sensor = ID_GR; 19442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; 19542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; 19642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 19742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].version = sizeof(sensors_event_t); 19842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].sensor = ID_GY; 19942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; 20042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; 20142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 20242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); 20342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].sensor = ID_A; 20442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; 20542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].acceleration.status 20642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 20742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 20842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].version = sizeof(sensors_event_t); 20942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].sensor = ID_M; 21042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; 21142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; 21242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 21342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].version = sizeof(sensors_event_t); 21442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].sensor = ID_O; 21542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; 21642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].orientation.status 21742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 21842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 21942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[RotationVector] = &MPLSensor::rvHandler; 22042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[LinearAccel] = &MPLSensor::laHandler; 22142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Gravity] = &MPLSensor::gravHandler; 22242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Gyro] = &MPLSensor::gyroHandler; 22342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Accelerometer] = &MPLSensor::accelHandler; 22442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[MagneticField] = &MPLSensor::compassHandler; 22542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Orientation] = &MPLSensor::orienHandler; 22642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 22742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (int i = 0; i < numSensors; i++) 22842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDelays[i] = 30000000LLU; // 30 ms by default 22942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 23042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_serial_start(port) != INV_SUCCESS) { 23142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal Error : could not open MPL serial interface"); 23242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 23342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 23442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //initialize library parameters 23542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru initMPL(); 23642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 23742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //setup the FIFO contents 23842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru setupFIFO(); 23942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 24042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //we start the motion processing only when a sensor is enabled... 24142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //rv = inv_dmp_start(); 24242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGE_IF(rv != INV_SUCCESS, "Fatal error: could not start the DMP correctly. (code = %d)\n", rv); 24342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //dmp_started = true; 24442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 24542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 24642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 24742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 24842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 24942331858975144405f95243be8427084ee7d478dJean-Baptiste QueruMPLSensor::~MPLSensor() 25042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 25142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 25242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 25342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_dmp_stop() != INV_SUCCESS) { 25442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGD("Error: could not stop the DMP correctly.\n"); 25542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 25642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 25742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_dmp_close() != INV_SUCCESS) { 25842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGD("Error: could not close the DMP"); 25942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 26042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 26142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_serial_stop() != INV_SUCCESS) { 26242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGD("Error : could not close the serial port"); 26342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 26442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 26542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 26642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 26742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* clear any data from our various filehandles */ 26842331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::clearIrqData(bool* irq_set) 26942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 27042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru unsigned int i; 27142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int nread; 27242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru struct mpuirq_data irqdata; 27342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 27442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru poll(mPollFds, ARRAY_SIZE(mPollFds), 0); //check which ones need to be cleared 27542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 27642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (i = 0; i < ARRAY_SIZE(mPollFds); i++) { 27742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int cur_fd = mPollFds[i].fd; 27842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int j = 0; 27942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mPollFds[i].revents & POLLIN) { 28042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru nread = read(cur_fd, &irqdata, sizeof(irqdata)); 28142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (nread > 0) { 28242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru irq_set[i] = true; 28342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGV_IF(EXTRA_VERBOSE, "irq: %d %d (%d)", i, irqdata.interruptcount, j++); 28442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 28542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 28642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[i].revents = 0; 28742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 28842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 28942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 29042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* set the power states of the various sensors based on the bits set in the 29142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * enabled_sensors parameter. 29242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * this function modifies globalish state variables. It must be called with the mMplMutex held. */ 29342331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::setPowerStates(int enabled_sensors) 29442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 29542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 29642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru bool irq_set[5] = { false, false, false, false, false }; 29742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 29842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV(" enabled_sensors: %d dmp_started: %d", enabled_sensors, mDmpStarted); 29942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 30042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru do { 30142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 30242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { 30342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask = ALL_MPL_SENSORS_NP; 30442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 30542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 30642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 30742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) { 30842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask = 0; 30942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 31042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 31142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 31242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (GY_ENABLED) { 31342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask |= INV_THREE_AXIS_GYRO; 31442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 31542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; 31642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 31742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 31842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (A_ENABLED) { 31942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask |= (INV_THREE_AXIS_ACCEL); 32042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 32142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL); 32242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 32342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 32442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (M_ENABLED) { 32542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask |= INV_THREE_AXIS_COMPASS; 32642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 32742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS); 32842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 32942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 33042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } while (0); 33142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 33242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //record the new sensor state 33342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t rv; 33442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 33542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru long sen_mask = mLocalSensorMask & mMasterSensorMask; 33642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 33742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru bool changing_sensors = ((inv_get_dl_config()->requested_sensors 33842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru != sen_mask) && (sen_mask != 0)); 33942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru bool restart = (!mDmpStarted) && (sen_mask != 0); 34042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 34142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (changing_sensors || restart) { 34242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 34342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart); 34442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 34542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mDmpStarted) { 34642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_dmp_stop(); 34742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clearIrqData(irq_set); 34842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDmpStarted = false; 34942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 35042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 35142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (sen_mask != inv_get_dl_config()->requested_sensors) { 35242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("inv_set_mpu_sensors: %lx", sen_mask); 35342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv = inv_set_mpu_sensors(sen_mask); 35442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF( 35542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv != INV_SUCCESS, 35642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "error: unable to set MPL sensor power states (sens=%ld retcode = %d)", 35742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru sen_mask, rv); 35842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 35942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 36042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS)) 36142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL))) 36242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru && ((sen_mask & INV_DMP_PROCESSOR) == 0)) { 36342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ"); 36442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mUseTimerirq = true; 36542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 36642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mUseTimerirq) { 36742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 36842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clearIrqData(irq_set); 36942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 37042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ"); 37142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mUseTimerirq = false; 37242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 37342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 37442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (!mDmpStarted) { 37542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("Starting DMP"); 37642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv = inv_dmp_start(); 37742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(rv != INV_SUCCESS, "unable to start dmp"); 37842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDmpStarted = true; 37942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 38042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 38142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 38242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //check if we should stop the DMP 38342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mDmpStarted && (sen_mask == 0)) { 38442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("Stopping DMP"); 38542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv = inv_dmp_stop(); 38642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)", 38742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv); 38842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mUseTimerirq) { 38942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 39042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 39142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clearIrqData(irq_set); 39242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mHaveGoodMpuCal) { 39342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv = inv_store_calibration(); 39442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(rv != INV_SUCCESS, 39542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "error: unable to store MPL calibration file"); 39642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHaveGoodMpuCal = false; 39742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 39842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 39942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDmpStarted = false; 40042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollTime = -1; 40142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mCurFifoRate = -1; 40242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 40342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 40442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 40542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 40642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/** 40742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * container function for all the calls we make once to set up the MPL. 40842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 40942331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::initMPL() 41042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 41142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 41242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t result; 41342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru unsigned short bias_update_mask = 0xFFFF; 41442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru struct mldl_cfg *mldl_cfg; 41542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 41642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_dmp_open() != INV_SUCCESS) { 41742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal Error : could not open DMP correctly.\n"); 41842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 41942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 42042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work 42142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(result != INV_SUCCESS, 42242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "Fatal Error : could not set enabled sensors."); 42342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 42442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_load_calibration() != INV_SUCCESS) { 42542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("could not open MPL calibration file"); 42642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 42742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 42842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //check for the 9axis fusion library: if available load it and start 9x 42942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru void* h_dmp_lib=dlopen("libmpl.so", RTLD_LAZY); 43042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if(h_dmp_lib) { 43142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru const char* error; 43242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t (*fp_inv_enable_9x_fusion)() = 43342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion"); 43442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if((error = dlerror()) != NULL) { 43542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("%s", error); 43642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) { 43742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE( "Warning : 9 axis sensor fusion not available " 43842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "- No compass detected.\n"); 43942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 44042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 44142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru const char* error = dlerror(); 44242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("libmpl.so not found, 9x sensor fusion disabled (%s)",error); 44342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 44442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 44542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mldl_cfg = inv_get_dl_config(); 44642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 44742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) { 44842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Error : Bias update function could not be set.\n"); 44942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 45042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 45142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_set_motion_interrupt(1) != INV_SUCCESS) { 45242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Error : could not set motion interrupt"); 45342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 45442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 45542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_set_fifo_interrupt(1) != INV_SUCCESS) { 45642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Error : could not set fifo interrupt"); 45742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 45842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 45942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_set_fifo_rate(6); 46042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 46142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_set_fifo_rate returned %d\n", result); 46242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 46342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 46442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mMpuAccuracy = SENSOR_STATUS_ACCURACY_MEDIUM; 46542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru setupCallbacks(); 46642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 46742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 46842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 46942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/** setup the fifo contents. 47042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 47142331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::setupFIFO() 47242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 47342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 47442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t result; 47542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 47642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_accel(INV_ALL, INV_32_BIT); 47742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 47842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_accel returned %d\n", result); 47942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 48042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 48142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_quaternion(INV_32_BIT); 48242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 48342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_quaternion returned %d\n", result); 48442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 48542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 48642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_linear_accel(INV_ALL, INV_32_BIT); 48742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 48842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_linear_accel returned %d\n", result); 48942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 49042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 49142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_linear_accel_in_world(INV_ALL, INV_32_BIT); 49242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 49342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_linear_accel_in_world returned %d\n", 49442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result); 49542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 49642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 49742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_gravity(INV_ALL, INV_32_BIT); 49842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 49942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_gravity returned %d\n", result); 50042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 50142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 50242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_gyro(INV_ALL, INV_32_BIT); 50342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 50442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_gyro returned %d\n", result); 50542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 50642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 50742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 50842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 50942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/** 51042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * set up the callbacks that we use in all cases (outside of gestures, etc) 51142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 51242331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::setupCallbacks() 51342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 51442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 51542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) { 51642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Error : Motion callback could not be set.\n"); 51742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 51842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 51942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 52042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) { 52142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Error : Processed data callback could not be set."); 52242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 52342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 52442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 52542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 52642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/** 52742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * handle the motion/no motion output from the MPL. 52842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 52942331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::cbOnMotion(uint16_t val) 53042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 53142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 53242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //after the first no motion, the gyro should be calibrated well 53342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (val == 2) { 53442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mMpuAccuracy = SENSOR_STATUS_ACCURACY_HIGH; 53542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled & (1 << MPLSensor::Gyro)) { 53642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //if gyros are on and we got a no motion, set a flag 53742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru // indicating that the cal file can be written. 53842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHaveGoodMpuCal = true; 53942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 54042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 54142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 54242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return; 54342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 54442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 54542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 54642331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::cbProcData() 54742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 54842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mNewData = 1; 54942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mSampleCount++; 55042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGV_IF(EXTRA_VERBOSE, "new data (%d)", sampleCount); 55142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 55242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 55342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru//these handlers transform mpl data into one of the Android sensor types 55442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru// scaling and coordinate transforms should be done in the handlers 55542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 55642331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask, 55742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 55842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 55942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 56042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 56142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_GYROS, s->gyro.v); 56242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0; 56342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0; 56442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0; 56542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.status = mMpuAccuracy; 56642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 56742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 56842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 56942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 57042331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask, 57142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 57242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 57342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //VFUNC_LOG; 57442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 57542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_ACCELS, s->acceleration.v); 57642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //res = inv_get_accel_float(s->acceleration.v); 57742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->acceleration.v[0] = s->acceleration.v[0] * 9.81; 57842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->acceleration.v[1] = s->acceleration.v[1] * 9.81; 57942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->acceleration.v[2] = s->acceleration.v[2] * 9.81; 58042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGV_IF(EXTRA_VERBOSE, "accel data: %f %f %f", s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2]); 58142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; 58242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 58342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 58442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 58542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 58642331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::estimateCompassAccuracy() 58742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 58842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 58942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int rv; 59042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 59142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_compass_accuracy(&rv); 59242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(res != INV_SUCCESS, "error returned from inv_get_compass_accuracy"); 59342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 59442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return rv; 59542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 59642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 59742331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask, 59842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 59942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 60042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 60142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res, res2; 60242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float bias_error[3]; 60342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float total_be; 60442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru static int bias_error_settled = 0; 60542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 60642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v); 60742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 60842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res != INV_SUCCESS) { 60942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGD( 61042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d", 61142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res); 61242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 61342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 61442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->magnetic.status = estimateCompassAccuracy(); 61542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 61642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 61742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 61842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 61942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 62042331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask, 62142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 62242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 62342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 62442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float quat[4]; 62542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float norm = 0; 62642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float ang = 0; 62742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t r; 62842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 62942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru r = inv_get_float_array(INV_QUATERNION, quat); 63042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 63142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (r != INV_SUCCESS) { 63242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask &= ~(1 << index); 63342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return; 63442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 63542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 63642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 63742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 63842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3] 63942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru + FLT_EPSILON; 64042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 64142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (norm > 1.0f) { 64242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //renormalize 64342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru norm = sqrtf(norm); 64442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float inv_norm = 1.0f / norm; 64542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[1] = quat[1] * inv_norm; 64642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[2] = quat[2] * inv_norm; 64742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[3] = quat[3] * inv_norm; 64842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 64942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 65042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (quat[0] < 0.0) { 65142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[1] = -quat[1]; 65242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[2] = -quat[2]; 65342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[3] = -quat[3]; 65442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 65542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 65642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[0] = quat[1]; 65742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[1] = quat[2]; 65842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[2] = quat[3]; 65942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 66042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.status 66142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = ((mMpuAccuracy < estimateCompassAccuracy()) ? mMpuAccuracy 66242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru : estimateCompassAccuracy()); 66342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 66442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 66542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask, 66642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 66742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 66842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 66942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 67042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_LINEAR_ACCELERATION, s->gyro.v); 67142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[0] *= 9.81; 67242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[1] *= 9.81; 67342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[2] *= 9.81; 67442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.status = mMpuAccuracy; 67542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 67642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 67742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 67842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 67942331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask, 68042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 68142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 68242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 68342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 68442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_GRAVITY, s->gyro.v); 68542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[0] *= 9.81; 68642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[1] *= 9.81; 68742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[2] *= 9.81; 68842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.status = mMpuAccuracy; 68942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 69042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 69142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 69242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 69342331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::calcOrientationSensor(float *R, float *values) 69442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 69542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float tmp; 69642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 69742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //Azimuth 69842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) { 69942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[0] = (float) atan2f(-R[3], R[0]); 70042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 70142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[0] = (float) atan2f(R[1], R[4]); 70242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 70342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[0] *= 57.295779513082320876798154814105f; 70442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (values[0] < 0) { 70542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[0] += 360.0f; 70642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 70742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //Pitch 70842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru tmp = R[7]; 70942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (tmp > 1.0f) 71042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru tmp = 1.0f; 71142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (tmp < -1.0f) 71242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru tmp = -1.0f; 71342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[1] = -asinf(tmp) * 57.295779513082320876798154814105f; 71442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (R[8] < 0) { 71542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[1] = 180.0f - values[1]; 71642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 71742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (values[1] > 180.0f) { 71842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[1] -= 360.0f; 71942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 72042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //Roll 72142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if ((R[7] > 0.7071067f)) { 72242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[2] = (float) atan2f(R[6], R[7]); 72342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 72442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[2] = (float) atan2f(R[6], R[8]); 72542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 72642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 72742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[2] *= 57.295779513082320876798154814105f; 72842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (values[2] > 90.0f) { 72942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[2] = 180.0f - values[2]; 73042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 73142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (values[2] < -90.0f) { 73242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[2] = -180.0f - values[2]; 73342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 73442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 73542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 73642331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::orienHandler(sensors_event_t* s, uint32_t* pending_mask, 73742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) //note that this is the handler for the android 'orientation' sensor, not the mpl orientation output 73842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 73942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 74042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 74142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float euler[3]; 74242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float heading[1]; 74342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float rot_mat[9]; 74442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 74542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat); 74642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 74742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //ComputeAndOrientation(heading[0], euler, s->orientation.v); 74842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru calcOrientationSensor(rot_mat, s->orientation.v); 74942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 75042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->orientation.status 75142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = ((mMpuAccuracy < estimateCompassAccuracy()) ? mMpuAccuracy 75242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru : estimateCompassAccuracy()); 75342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 75442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 75542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 75642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru else 75742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGD("orien_handler: data not valid (%d)", (int) res); 75842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 75942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 76042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 76142331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::enable(int32_t handle, int en) 76242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 76342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 76442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("handle : %d en: %d", handle, en); 76542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 76642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int what = -1; 76742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 76842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru switch (handle) { 76942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_A: 77042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Accelerometer; 77142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 77242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_M: 77342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = MagneticField; 77442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 77542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_O: 77642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Orientation; 77742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 77842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GY: 77942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gyro; 78042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 78142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GR: 78242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gravity; 78342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 78442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_RV: 78542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = RotationVector; 78642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 78742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_LA: 78842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = LinearAccel; 78942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 79042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru default: //this takes care of all the gestures 79142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = handle; 79242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 79342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 79442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 79542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (uint32_t(what) >= numSensors) 79642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 79742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 79842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int newState = en ? 1 : 0; 79942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int err = 0; 80042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF((uint32_t(newState) << what) != (mEnabled & (1 << what)), 80142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "sensor state change what=%d", what); 80242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 80342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 80442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { 80542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru uint32_t sensor_type; 80642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru short flags = newState; 80742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mEnabled &= ~(1 << what); 80842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mEnabled |= (uint32_t(flags) << what); 80942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "mEnabled = %x", mEnabled); 81042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru setPowerStates(mEnabled); 81142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 81242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (!newState) 81342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru update_delay(); 81442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return err; 81542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 81642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 81742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return err; 81842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 81942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 82042331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::setDelay(int32_t handle, int64_t ns) 82142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 82242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 82342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru /* LOGV_IF(EXTRA_VERBOSE, */ 82442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE(" setDelay handle: %d rate %d", handle, (int) (ns / 1000000LL)); 82542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int what = -1; 82642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru switch (handle) { 82742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_A: 82842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Accelerometer; 82942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 83042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_M: 83142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = MagneticField; 83242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 83342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_O: 83442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Orientation; 83542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 83642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GY: 83742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gyro; 83842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 83942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GR: 84042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gravity; 84142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 84242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_RV: 84342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = RotationVector; 84442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 84542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_LA: 84642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = LinearAccel; 84742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 84842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru default: 84942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = handle; 85042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 85142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 85242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 85342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (uint32_t(what) >= numSensors) 85442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 85542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 85642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (ns < 0) 85742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 85842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 85942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 86042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDelays[what] = ns; 86142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 86242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return update_delay(); 86342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 86442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 86542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::update_delay() 86642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 86742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 86842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int rv = 0; 86942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru bool irq_set[5]; 87042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 87142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 87242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 87342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled) { 87442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru uint64_t wanted = -1LLU; 87542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (int i = 0; i < numSensors; i++) { 87642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled & (1 << i)) { 87742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru uint64_t ns = mDelays[i]; 87842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru wanted = wanted < ns ? wanted : ns; 87942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 88042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 88142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 88242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns 88342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (wanted < 10000000LLU) { 88442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru wanted = 10000000LLU; 88542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 88642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 88742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1 88842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru : 0); //mpu fifo rate is in increments of 5ms 88942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (rate == 0) //KLP disallow fifo rate 0 89042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rate = 1; 89142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 89242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (rate != mCurFifoRate) { 89342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("set fifo rate: %d %llu", rate, wanted); 89442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; // = inv_dmp_stop(); 89542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_set_fifo_rate(rate); 89642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(res != INV_SUCCESS, "error setting FIFO rate"); 89742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 89842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //res = inv_dmp_start(); 89942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGE_IF(res != INV_SUCCESS, "error re-starting DMP"); 90042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 90142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mCurFifoRate = rate; 90242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv = (res == INV_SUCCESS); 90342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 90442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 90542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) { 90642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mUseTimerirq) { 90742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 90842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clearIrqData(irq_set); 90942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_get_dl_config()->requested_sensors 91042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru == INV_THREE_AXIS_COMPASS) { 91142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, 91242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru (unsigned long) (wanted / 1000000LLU)); 91342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", 91442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru (int) (wanted / 1000000LLU)); 91542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 91642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, 91742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru (unsigned long) inv_get_sample_step_size_ms()); 91842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", 91942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru (int) inv_get_sample_step_size_ms()); 92042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 92142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 92242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 92342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 92442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 92542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 92642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return rv; 92742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 92842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 92942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* return the current time in nanoseconds */ 93042331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint64_t MPLSensor::now_ns(void) 93142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 93242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //FUNC_LOG; 93342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru struct timespec ts; 93442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 93542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clock_gettime(CLOCK_MONOTONIC, &ts); 93642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGV("Time %lld", (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec); 93742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; 93842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 93942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 94042331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::readEvents(sensors_event_t* data, int count) 94142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 94242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //VFUNC_LOG; 94342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int i; 94442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru bool irq_set[5] = { false, false, false, false, false }; 94542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t rv; 94642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (count < 1) 94742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 94842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int numEventReceived = 0; 94942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 95042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clearIrqData(irq_set); 95142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 95242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 95342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mDmpStarted) { 95442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGV_IF(EXTRA_VERBOSE, "Update Data"); 95542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv = inv_update_data(); 95642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv); 95742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 95842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 95942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru else { 96042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //probably just one extra read after shutting down 96142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, 96242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "MPLSensor::readEvents called, but there's nothing to do."); 96342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 96442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 96542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 96642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 96742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (!mNewData) { 96842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "no new data"); 96942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return 0; 97042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 97142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mNewData = 0; 97242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int64_t tt = now_ns(); 97342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 97442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (int i = 0; i < numSensors; i++) { 97542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled & (1 << i)) { 97642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i, 97742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru &mPendingMask, i); 97842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[i].timestamp = tt; 97942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 98042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 98142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 98242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (int j = 0; count && mPendingMask && j < numSensors; j++) { 98342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mPendingMask & (1 << j)) { 98442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingMask &= ~(1 << j); 98542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled & (1 << j)) { 98642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *data++ = mPendingEvents[j]; 98742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru count--; 98842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru numEventReceived++; 98942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 99042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 99142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 99242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 99342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 99442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 99542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return numEventReceived; 99642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 99742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 99842331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getFd() const 99942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 100042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("MPLSensor::getFd returning %d", data_fd); 100142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return data_fd; 100242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 100342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 100442331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getAccelFd() const 100542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 100642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("MPLSensor::getAccelFd returning %d", accel_fd); 100742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return accel_fd; 100842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 100942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 101042331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getTimerFd() const 101142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 101242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("MPLSensor::getTimerFd returning %d", timer_fd); 101342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return timer_fd; 101442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 101542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 101642331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getPowerFd() const 101742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 101842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int hdl = (int) inv_get_serial_handle(); 101942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("MPLSensor::getPowerFd returning %d", hdl); 102042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return hdl; 102142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 102242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 102342331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getPollTime() 102442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 102542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return mPollTime; 102642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 102742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 102842331858975144405f95243be8427084ee7d478dJean-Baptiste Querubool MPLSensor::hasPendingEvents() const 102942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 103042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //if we are using the polling workaround, force the main loop to check for data every time 103142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return (mPollTime != -1); 103242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 103342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 103442331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::handlePowerEvent() 103542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 103642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 103742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mpuirq_data irqd; 103842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 103942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int fd = (int) inv_get_serial_handle(); 104042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru read(fd, &irqd, sizeof(irqd)); 104142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 104242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) { 104342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //going to sleep 104442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru sleepEvent(); 104542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) { 104642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //waking up 104742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru wakeEvent(); 104842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 104942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 105042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(fd, MPU_PM_EVENT_HANDLED, 0); 105142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 105242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 105342331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::sleepEvent() 105442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 105542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 105642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 105742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled != 0) { 105842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mForceSleep = true; 105942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mOldEnabledMask = mEnabled; 106042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru setPowerStates(0); 106142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 106242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 106342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 106442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 106542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::wakeEvent() 106642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 106742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 106842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 106942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mForceSleep) { 107042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru setPowerStates((mOldEnabledMask | mEnabled)); 107142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 107242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mForceSleep = false; 107342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 107442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 1075