MPLSensor.cpp revision 60c042a376b34a7f4343752b96c1bc0bf4da31cc
142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* 2895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Copyright (C) 2011 Invensense, Inc. 3895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 4895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Licensed under the Apache License, Version 2.0 (the "License"); 5895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * you may not use this file except in compliance with the License. 6895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * You may obtain a copy of the License at 7895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 8895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * http://www.apache.org/licenses/LICENSE-2.0 9895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 10895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Unless required by applicable law or agreed to in writing, software 11895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * distributed under the License is distributed on an "AS IS" BASIS, 12895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * See the License for the specific language governing permissions and 14895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * limitations under the License. 15895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 16895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 17895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall//#define LOG_NDEBUG 0 18895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall//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> 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 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 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "math.h" 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml.h" 39895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFO.h" 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsl.h" 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlos.h" 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml_mputest.h" 4342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "ml_stored_data.h" 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl_cfg.h" 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl.h" 4642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mpu.h" 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "accel.h" 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "compass.h" 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "kernel/timerirq.h" 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "kernel/mpuirq.h" 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "kernel/slaveirq.h" 5342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallextern "C" { 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsupervisor.h" 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 5742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlcontrol.h" 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "sensor_params.h" 6042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define EXTRA_VERBOSE (0) 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall//#define FUNC_LOG ALOGV("%s", __PRETTY_FUNCTION__) 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define FUNC_LOG 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define VFUNC_LOG ALOGV_IF(EXTRA_VERBOSE, "%s", __PRETTY_FUNCTION__) 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* this mask must turn on only the sensors that are present and managed by the MPL */ 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_COMPASS | INV_THREE_AXIS_GYRO) 6742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define CALL_MEMBER_FN(pobject,ptrToMember) ((pobject)->*(ptrToMember)) 69c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 70895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/******************************************/ 71986d38918eb72f00a67189a942da432df17e504eKevin Powell 72895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* Base values for the sensor list, these need to be in the order defined in MPLSensor.h */ 73986d38918eb72f00a67189a942da432df17e504eKevin Powellstatic struct sensor_t sSensorList[] = 74895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { { "MPL Gyroscope", "Invensense", 1, 75895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSORS_GYROSCOPE_HANDLE, 76895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, { } }, 77895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { "MPL Accelerometer", "Invensense", 1, 78895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSORS_ACCELERATION_HANDLE, 79895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, { } }, 80895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { "MPL Magnetic Field", "Invensense", 1, 81895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSORS_MAGNETIC_FIELD_HANDLE, 82895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, { } }, 83895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { "MPL Orientation", "Invensense", 1, 84895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSORS_ORIENTATION_HANDLE, 85895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, { } }, 86895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { "MPL Rotation Vector", "Invensense", 1, 87895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSORS_ROTATION_VECTOR_HANDLE, 88895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, { } }, 89895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { "MPL Linear Acceleration", "Invensense", 1, 90895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSORS_LINEAR_ACCEL_HANDLE, 91895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, { } }, 92895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { "MPL Gravity", "Invensense", 1, 93895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSORS_GRAVITY_HANDLE, 94895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, { } }, 95986d38918eb72f00a67189a942da432df17e504eKevin Powell}; 96986d38918eb72f00a67189a942da432df17e504eKevin Powell 97895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic unsigned long long irq_timestamp = 0; 98895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* *************************************************************************** 99895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * MPL interface misc. 100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall//static pointer to the object that will handle callbacks 102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic MPLSensor* gMPLSensor = NULL; 10342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* we need to pass some callbacks to the MPL. The mpl is a C library, so 105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * wrappers for the C++ callback implementations are required. 106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 10742331858975144405f95243be8427084ee7d478dJean-Baptiste Queruextern "C" { 108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall//callback wrappers go here 109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid mot_cb_wrapper(uint16_t val) 110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (gMPLSensor) { 112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gMPLSensor->cbOnMotion(val); 113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 11642331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid procData_cb_wrapper() 11742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(gMPLSensor) { 119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gMPLSensor->cbProcData(); 12042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 12142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 12242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} //end of extern C 124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 12542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid setCallbackObject(MPLSensor* gbpt) 12642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gMPLSensor = gbpt; 12842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 12942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 13042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/***************************************************************************** 132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * sensor class implementation 133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 13442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define GY_ENABLED ((1<<ID_GY) & enabled_sensors) 136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define A_ENABLED ((1<<ID_A) & enabled_sensors) 137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define O_ENABLED ((1<<ID_O) & enabled_sensors) 138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define M_ENABLED ((1<<ID_M) & enabled_sensors) 139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define LA_ENABLED ((1<<ID_LA) & enabled_sensors) 140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define GR_ENABLED ((1<<ID_GR) & enabled_sensors) 141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define RV_ENABLED ((1<<ID_RV) & enabled_sensors) 142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 143895401859313187f15a800e62d43e6bcbf48fadaJP AbgrallMPLSensor::MPLSensor() : 144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall SensorBase(NULL, NULL), 14560c042a376b34a7f4343752b96c1bc0bf4da31ccJP Abgrall mNewData(0), 146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mDmpStarted(false), 147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mMasterSensorMask(INV_ALL_SENSORS), 148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1), 149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mCurFifoRate(-1), mHaveGoodMpuCal(false), mHaveGoodCompassCal(false), 150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mUseTimerIrqAccel(false), mUsetimerIrqCompass(true), 151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mUseTimerirq(false), mSampleCount(0), 152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mEnabled(0), mPendingMask(0) 153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 15542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t rv; 156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int mpu_int_fd, i; 15742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru char *port = NULL; 15842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors); 16042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 161be6c0220038706c329ea7e40bf07b1fa130977c8Rosa pthread_mutex_init(&mMplMutex, NULL); 162be6c0220038706c329ea7e40bf07b1fa130977c8Rosa 163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mForceSleep = false; 16442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* used for identifying whether 9axis is enabled or not */ 166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* this variable will be changed in initMPL() when libmpl is loaded */ 167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* sensor list will be changed based on this variable */ 168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mNineAxisEnabled = false; 169986d38918eb72f00a67189a942da432df17e504eKevin Powell 170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < ARRAY_SIZE(mPollFds); i++) { 171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollFds[i].fd = -1; 172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollFds[i].events = 0; 17342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 17442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 17642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mpu_int_fd = open("/dev/mpuirq", O_RDWR); 178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mpu_int_fd == -1) { 179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("could not open the mpu irq device node"); 180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK); 182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ioctl(mpu_int_fd, MPUIRQ_SET_TIMEOUT, 0); 183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mIrqFds.add(MPUIRQ_FD, mpu_int_fd); 184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollFds[MPUIRQ_FD].fd = mpu_int_fd; 185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollFds[MPUIRQ_FD].events = POLLIN; 186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 187c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accel_fd = open("/dev/accelirq", O_RDWR); 189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accel_fd == -1) { 190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("could not open the accel irq device node"); 191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fcntl(accel_fd, F_SETFL, O_NONBLOCK); 193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ioctl(accel_fd, SLAVEIRQ_SET_TIMEOUT, 0); 194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mIrqFds.add(ACCELIRQ_FD, accel_fd); 195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollFds[ACCELIRQ_FD].fd = accel_fd; 196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollFds[ACCELIRQ_FD].events = POLLIN; 197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 19842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall timer_fd = open("/dev/timerirq", O_RDWR); 200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (timer_fd == -1) { 201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("could not open the timer irq device node"); 20242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fcntl(timer_fd, F_SETFL, O_NONBLOCK); 204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ioctl(timer_fd, TIMERIRQ_SET_TIMEOUT, 0); 205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mIrqFds.add(TIMERIRQ_FD, timer_fd); 206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollFds[TIMERIRQ_FD].fd = timer_fd; 207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollFds[TIMERIRQ_FD].events = POLLIN; 20842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 20942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data_fd = mpu_int_fd; 21142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((accel_fd == -1) && (timer_fd != -1)) { 213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //no accel irq and timer available 214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mUseTimerIrqAccel = true; 215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGD("MPLSensor falling back to timerirq for accel data"); 21642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 21742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 21842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru memset(mPendingEvents, 0, sizeof(mPendingEvents)); 21942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 22042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[RotationVector].version = sizeof(sensors_event_t); 22142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[RotationVector].sensor = ID_RV; 22242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; 22342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 22442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); 22542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].sensor = ID_LA; 22642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; 22742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 22842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].version = sizeof(sensors_event_t); 22942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].sensor = ID_GR; 23042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; 23142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 23242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].version = sizeof(sensors_event_t); 23342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].sensor = ID_GY; 23442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; 23542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 23642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); 23742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].sensor = ID_A; 23842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; 23942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 24042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].version = sizeof(sensors_event_t); 24142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].sensor = ID_M; 24242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; 243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; 24442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 24542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].version = sizeof(sensors_event_t); 24642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].sensor = ID_O; 24742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; 24842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].orientation.status 24942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 25042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 25142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[RotationVector] = &MPLSensor::rvHandler; 25242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[LinearAccel] = &MPLSensor::laHandler; 25342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Gravity] = &MPLSensor::gravHandler; 25442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Gyro] = &MPLSensor::gyroHandler; 25542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Accelerometer] = &MPLSensor::accelHandler; 25642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[MagneticField] = &MPLSensor::compassHandler; 25742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Orientation] = &MPLSensor::orienHandler; 25842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (int i = 0; i < numSensors; i++) 260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mDelays[i] = 30000000LLU; // 30 ms by default 261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_serial_start(port) != INV_SUCCESS) { 263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal Error : could not open MPL serial interface"); 26442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 26542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //initialize library parameters 267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall initMPL(); 26842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //setup the FIFO contents 270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall setupFIFO(); 27142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //we start the motion processing only when a sensor is enabled... 273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //rv = inv_dmp_start(); 274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGE_IF(rv != INV_SUCCESS, "Fatal error: could not start the DMP correctly. (code = %d)\n", rv); 275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //dmp_started = true; 27642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 27842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 28042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 281895401859313187f15a800e62d43e6bcbf48fadaJP AbgrallMPLSensor::~MPLSensor() 282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_dmp_stop() != INV_SUCCESS) { 286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGW("Error: could not stop the DMP correctly.\n"); 28742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 28842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_dmp_close() != INV_SUCCESS) { 290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGW("Error: could not close the DMP"); 29142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 29242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_serial_stop() != INV_SUCCESS) { 294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGW("Error : could not close the serial port"); 29542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_destroy(&mMplMutex); 29842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 29942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* clear any data from our various filehandles */ 301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::clearIrqData(bool* irq_set) 30242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned int i; 304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int nread; 305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mpuirq_data irqdata; 306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall poll(mPollFds, ARRAY_SIZE(mPollFds), 0); //check which ones need to be cleared 308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < ARRAY_SIZE(mPollFds); i++) { 310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int cur_fd = mPollFds[i].fd; 311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int j = 0; 312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mPollFds[i].revents & POLLIN) { 313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall nread = read(cur_fd, &irqdata, sizeof(irqdata)); 314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (nread > 0) { 315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall irq_set[i] = true; 316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall irq_timestamp = irqdata.irqtime; 317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV_IF(EXTRA_VERBOSE, "irq: %d %d (%d)", i, irqdata.interruptcount, j++); 318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollFds[i].revents = 0; 32142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 32242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 32342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* set the power states of the various sensors based on the bits set in the 325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * enabled_sensors parameter. 326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * this function modifies globalish state variables. It must be called with the mMplMutex held. */ 327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::setPowerStates(int enabled_sensors) 32842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bool irq_set[5] = { false, false, false, false, false }; 33142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV(" setPowerStates: %d dmp_started: %d", enabled_sensors, mDmpStarted); 33342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall do { 33542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { 337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask = ALL_MPL_SENSORS_NP; 338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 33942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 34042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) { 342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask = 0; 343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 34442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 34542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (GY_ENABLED) { 347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask |= INV_THREE_AXIS_GYRO; 348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; 35042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 35142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (A_ENABLED) { 353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask |= (INV_THREE_AXIS_ACCEL); 354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL); 35642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 35742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (M_ENABLED) { 359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask |= INV_THREE_AXIS_COMPASS; 360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS); 362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 36342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } while (0); 36542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //record the new sensor state 367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t rv; 36842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long sen_mask = mLocalSensorMask & mMasterSensorMask; 370c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bool changing_sensors = ((inv_get_dl_config()->requested_sensors 372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall != sen_mask) && (sen_mask != 0)); 373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bool restart = (!mDmpStarted) && (sen_mask != 0); 374c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (changing_sensors || restart) { 376c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart); 378c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mDmpStarted) { 380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_dmp_stop(); 381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clearIrqData(irq_set); 382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mDmpStarted = false; 383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 38442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sen_mask != inv_get_dl_config()->requested_sensors) { 386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("setPowerStates: %lx", sen_mask); 387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = inv_set_mpu_sensors(sen_mask); 388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(rv != INV_SUCCESS, 389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "error: unable to set MPL sensor power states (sens=%ld retcode = %d)", 390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sen_mask, rv); 391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 392c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS)) 394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL))) 395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall && ((sen_mask & INV_DMP_PROCESSOR) == 0)) { 396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ"); 397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mUseTimerirq = true; 398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mUseTimerirq) { 400895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clearIrqData(irq_set); 402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ"); 404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mUseTimerirq = false; 405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 40642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!mDmpStarted) { 408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mHaveGoodMpuCal || mHaveGoodCompassCal) { 409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = inv_store_calibration(); 410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(rv != INV_SUCCESS, 411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "error: unable to store MPL calibration file"); 412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mHaveGoodMpuCal = false; 413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mHaveGoodCompassCal = false; 414895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("Starting DMP"); 416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = inv_dmp_start(); 417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp"); 418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mDmpStarted = true; 419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 42042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 42142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //check if we should stop the DMP 423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mDmpStarted && (sen_mask == 0)) { 424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("Stopping DMP"); 425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = inv_dmp_stop(); 426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)", 427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv); 428895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mUseTimerirq) { 429895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 43042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clearIrqData(irq_set); 432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mDmpStarted = false; 434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollTime = -1; 435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mCurFifoRate = -1; 43642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 43742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 438c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 43942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * container function for all the calls we make once to set up the MPL. 442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::initMPL() 444c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall{ 445895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 446895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 447895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short bias_update_mask = 0xFFFF; 448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg; 44942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_dmp_open() != INV_SUCCESS) { 451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal Error : could not open DMP correctly.\n"); 45242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 45342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work 455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(result != INV_SUCCESS, 456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "Fatal Error : could not set enabled sensors."); 45742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_load_calibration() != INV_SUCCESS) { 459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("could not open MPL calibration file"); 460895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 461895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //check for the 9axis fusion library: if available load it and start 9x 463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void* h_dmp_lib=dlopen("libinvensense_mpl.so", RTLD_NOW); 464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(h_dmp_lib) { 465895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const char* error; 466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall error = dlerror(); 467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t (*fp_inv_enable_9x_fusion)() = 468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion"); 469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if((error = dlerror()) != NULL) { 470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("%s %s", error, "inv_enable_9x_fusion"); 471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) { 472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE( "Warning : 9 axis sensor fusion not available " 473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "- No compass detected.\n"); 474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* 9axis is loaded and enabled */ 476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* this variable is used for coming up with sensor list */ 477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mNineAxisEnabled = true; 478c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const char* error = dlerror(); 481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error); 48242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 48342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldl_cfg = inv_get_dl_config(); 48542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 486895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) { 487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Error : Bias update function could not be set.\n"); 488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 48942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 490895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_motion_interrupt(1) != INV_SUCCESS) { 491895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Error : could not set motion interrupt"); 492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 49342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_fifo_interrupt(1) != INV_SUCCESS) { 495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Error : could not set fifo interrupt"); 49642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 497895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_fifo_rate(6); 499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result); 50142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 50242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall setupCallbacks(); 50442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 50542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 50642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** setup the fifo contents. 508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 509895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::setupFIFO() 51042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 511895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 51342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 514895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_accel(INV_ALL, INV_32_BIT); 515895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_accel returned %d\n", result); 51742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 51842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 519895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_quaternion(INV_32_BIT); 520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_quaternion returned %d\n", result); 522895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 523c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_linear_accel(INV_ALL, INV_32_BIT); 525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 526895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_linear_accel returned %d\n", result); 52742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 52842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_linear_accel_in_world(INV_ALL, INV_32_BIT); 530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_linear_accel_in_world returned %d\n", 532895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result); 533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 53442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_gravity(INV_ALL, INV_32_BIT); 536895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 537895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_gravity returned %d\n", result); 538c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 53942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_gyro(INV_ALL, INV_32_BIT); 541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_gyro returned %d\n", result); 543895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 54442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 54542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 54642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 547895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 548895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * set up the callbacks that we use in all cases (outside of gestures, etc) 549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::setupCallbacks() 55142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 553895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) { 554895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Error : Motion callback could not be set.\n"); 55542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 55642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 55742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) { 559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Error : Processed data callback could not be set."); 56042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 56142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 562c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 56342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 564895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 565895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * handle the motion/no motion output from the MPL. 566895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::cbOnMotion(uint16_t val) 568c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall{ 569895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 570895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //after the first no motion, the gyro should be calibrated well 571895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (val == 2) { 572895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) { 573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //if gyros are on and we got a no motion, set a flag 574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // indicating that the cal file can be written. 575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mHaveGoodMpuCal = true; 576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 577895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 57842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return; 58042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 58142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 582c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 583895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::cbProcData() 584895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 585895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mNewData = 1; 586895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mSampleCount++; 587895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV_IF(EXTRA_VERBOSE, "new data (%d)", sampleCount); 588c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 589c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 590895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall//these handlers transform mpl data into one of the Android sensor types 591895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall// scaling and coordinate transforms should be done in the handlers 592895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 593895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask, 594895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 59542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 59642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 597895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 598895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_GYROS, s->gyro.v); 599895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0; 600895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0; 601895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0; 602895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 603895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 604895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 605895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 606895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask, 607895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 608895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 609895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //VFUNC_LOG; 610895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 611895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_ACCELS, s->acceleration.v); 612895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //res = inv_get_accel_float(s->acceleration.v); 613895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->acceleration.v[0] = s->acceleration.v[0] * 9.81; 614895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->acceleration.v[1] = s->acceleration.v[1] * 9.81; 615895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->acceleration.v[2] = s->acceleration.v[2] * 9.81; 616895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV_IF(EXTRA_VERBOSE, "accel data: %f %f %f", s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2]); 617895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 618895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 619895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 620895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 621895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::estimateCompassAccuracy() 622895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 623895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 624895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int rv; 625895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 626895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_compass_accuracy(&rv); 627895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(rv >= SENSOR_STATUS_ACCURACY_MEDIUM) { 628895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mHaveGoodCompassCal = true; 629895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 630895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(res != INV_SUCCESS, "error returned from inv_get_compass_accuracy"); 631c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 632895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return rv; 63342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 63442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 635895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask, 636895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 63742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 638c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall VFUNC_LOG; 639895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res, res2; 640895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float bias_error[3]; 641895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float total_be; 642895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static int bias_error_settled = 0; 64342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 644895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v); 64542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 646895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res != INV_SUCCESS) { 647895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGW( 648895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d", 649895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res); 65042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 651895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 652895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->magnetic.status = estimateCompassAccuracy(); 653895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 654895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 655895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 65642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 65742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 658895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask, 659895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 66042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 66142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 662895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float quat[4]; 663895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float norm = 0; 664895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float ang = 0; 665895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t r; 66642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 667895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall r = inv_get_float_array(INV_QUATERNION, quat); 66842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 669895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (r != INV_SUCCESS) { 670895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask &= ~(1 << index); 671895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return; 672c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } else { 673895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 674c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 67542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 676895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3] 677895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall + FLT_EPSILON; 67842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 679895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (norm > 1.0f) { 680895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //renormalize 681895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall norm = sqrtf(norm); 682895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float inv_norm = 1.0f / norm; 683895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[1] = quat[1] * inv_norm; 684895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[2] = quat[2] * inv_norm; 685895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[3] = quat[3] * inv_norm; 686895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 687c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 688895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (quat[0] < 0.0) { 689895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[1] = -quat[1]; 690895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[2] = -quat[2]; 691895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[3] = -quat[3]; 692c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 69342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 694895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[0] = quat[1]; 695895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[1] = quat[2]; 696895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[2] = quat[3]; 697895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 69842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 69942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 700895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask, 701895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 70242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 703c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall VFUNC_LOG; 704895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 705895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_LINEAR_ACCELERATION, s->gyro.v); 706895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[0] *= 9.81; 707895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[1] *= 9.81; 708895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[2] *= 9.81; 709895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 710895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 711895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 712895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 713895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask, 714895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 715895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 716895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall VFUNC_LOG; 717895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 718895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_GRAVITY, s->gyro.v); 719895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[0] *= 9.81; 720895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[1] *= 9.81; 721895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[2] *= 9.81; 722895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 723895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 724895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 72542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 726895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::calcOrientationSensor(float *R, float *values) 727895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 728895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float tmp; 729c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 730895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Azimuth 731895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) { 732895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[0] = (float) atan2f(-R[3], R[0]); 733895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 734895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[0] = (float) atan2f(R[1], R[4]); 735895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 736895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[0] *= 57.295779513082320876798154814105f; 737895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (values[0] < 0) { 738895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[0] += 360.0f; 739895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 740895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Pitch 741895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = R[7]; 742895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 1.0f) 743895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 1.0f; 744895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp < -1.0f) 745895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = -1.0f; 746895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[1] = -asinf(tmp) * 57.295779513082320876798154814105f; 747895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (R[8] < 0) { 748895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[1] = 180.0f - values[1]; 749895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 750895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (values[1] > 180.0f) { 751895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[1] -= 360.0f; 752895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 753895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Roll 754895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((R[7] > 0.7071067f)) { 755895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[2] = (float) atan2f(R[6], R[7]); 756895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 757895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[2] = (float) atan2f(R[6], R[8]); 75842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 75942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 760895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[2] *= 57.295779513082320876798154814105f; 761895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (values[2] > 90.0f) { 762895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[2] = 180.0f - values[2]; 763895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 764895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (values[2] < -90.0f) { 765895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[2] = -180.0f - values[2]; 766c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 76742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 76842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 769895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::orienHandler(sensors_event_t* s, uint32_t* pending_mask, 770895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) //note that this is the handler for the android 'orientation' sensor, not the mpl orientation output 77142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 772c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall VFUNC_LOG; 773895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 774895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float euler[3]; 775895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float heading[1]; 776895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float rot_mat[9]; 777c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 778895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat); 779c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 780895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ComputeAndOrientation(heading[0], euler, s->orientation.v); 781895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calcOrientationSensor(rot_mat, s->orientation.v); 782c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 783895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->orientation.status = estimateCompassAccuracy(); 784c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 785895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 786895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 787895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 788895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGW("orienHandler: data not valid (%d)", (int) res); 789c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 790c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 791c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 792895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::enable(int32_t handle, int en) 793895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 794895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 795895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("handle : %d en: %d", handle, en); 796c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 797895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int what = -1; 798c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 799c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall switch (handle) { 800c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_A: 801c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = Accelerometer; 802c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 803c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_M: 80442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = MagneticField; 80542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 80642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_O: 80742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Orientation; 80842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 80942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GY: 81042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gyro; 81142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 81242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GR: 81342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gravity; 81442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 81542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_RV: 81642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = RotationVector; 81742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 81842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_LA: 81942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = LinearAccel; 82042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 821c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall default: //this takes care of all the gestures 82242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = handle; 82342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 82442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 82542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 82642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (uint32_t(what) >= numSensors) 82742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 82842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 829c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall int newState = en ? 1 : 0; 830c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall int err = 0; 831895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV_IF((uint32_t(newState) << what) != (mEnabled & (1 << what)), 832895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // "sensor state change what=%d", what); 833c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 834895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 835c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { 836c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall uint32_t sensor_type; 837c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall short flags = newState; 838c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall mEnabled &= ~(1 << what); 839c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall mEnabled |= (uint32_t(flags) << what); 840895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "mEnabled = %x", mEnabled); 841895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall setPowerStates(mEnabled); 842895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 843895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!newState) 844895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall update_delay(); 845895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return err; 846895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 847895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 848c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall return err; 849c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 850c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 851c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrallint MPLSensor::setDelay(int32_t handle, int64_t ns) 852c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall{ 853895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 854895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, 855895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall " setDelay handle: %d rate %d", handle, (int) (ns / 1000000LL)); 856c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall int what = -1; 857c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall switch (handle) { 858c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_A: 859c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = Accelerometer; 860c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 861c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_M: 862c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = MagneticField; 863c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 864c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_O: 865c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = Orientation; 866c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 867c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_GY: 868c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = Gyro; 869c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 870c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_GR: 871c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = Gravity; 872c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 873c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_RV: 874c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = RotationVector; 875c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 876c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_LA: 877c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = LinearAccel; 878c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 879895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 880c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = handle; 881c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 882c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 883c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 884c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall if (uint32_t(what) >= numSensors) 885c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall return -EINVAL; 886c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 88742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (ns < 0) 88842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 88942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 890895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 89142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDelays[what] = ns; 892895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 893895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return update_delay(); 89442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 89542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 896895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::update_delay() 897895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 898895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 899895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int rv = 0; 900895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bool irq_set[5]; 90142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 902895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 90342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 90442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled) { 90542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru uint64_t wanted = -1LLU; 90642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (int i = 0; i < numSensors; i++) { 90742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled & (1 << i)) { 90842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru uint64_t ns = mDelays[i]; 90942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru wanted = wanted < ns ? wanted : ns; 91042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 91142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 91242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 913895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns 914895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (wanted < 10000000LLU) { 915895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall wanted = 10000000LLU; 91642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 91742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 918895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1 919895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall : 0); //mpu fifo rate is in increments of 5ms 920895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (rate == 0) //KLP disallow fifo rate 0 921895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rate = 1; 92242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 923895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (rate != mCurFifoRate) { 924895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGD("set fifo rate: %d %llu", rate, wanted); 925895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; // = inv_dmp_stop(); 926895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_set_fifo_rate(rate); 927895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate"); 92842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 929895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //res = inv_dmp_start(); 930895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGE_IF(res != INV_SUCCESS, "error re-starting DMP"); 931c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 932895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mCurFifoRate = rate; 933895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = (res == INV_SUCCESS); 934895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 93542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 936895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) { 937895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mUseTimerirq) { 938895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 939895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clearIrqData(irq_set); 940895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_dl_config()->requested_sensors 941895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall == INV_THREE_AXIS_COMPASS) { 942895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, 943895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (unsigned long) (wanted / 1000000LLU)); 944895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", 945895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (int) (wanted / 1000000LLU)); 94642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 947895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, 948895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (unsigned long) inv_get_sample_step_size_ms()); 949895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", 950895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (int) inv_get_sample_step_size_ms()); 95142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 95242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 95342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 95442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 95542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 956895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 957895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return rv; 95842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 95942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 960895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* return the current time in nanoseconds */ 961895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint64_t MPLSensor::now_ns(void) 96242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 963895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //FUNC_LOG; 964895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct timespec ts; 96542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 966895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clock_gettime(CLOCK_MONOTONIC, &ts); 967895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("Time %lld", (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec); 968895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; 969c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 970c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 971895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::readEvents(sensors_event_t* data, int count) 972c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall{ 973895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //VFUNC_LOG; 974895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i; 975895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bool irq_set[5] = { false, false, false, false, false }; 976895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t rv; 977895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (count < 1) 978895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return -EINVAL; 979c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall int numEventReceived = 0; 980c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 981895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clearIrqData(irq_set); 98242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 983895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 984895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mDmpStarted) { 985895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV_IF(EXTRA_VERBOSE, "Update Data"); 986895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = inv_update_data(); 987895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv); 988c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 989c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 990895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else { 991895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //probably just one extra read after shutting down 992895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, 993895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "MPLSensor::readEvents called, but there's nothing to do."); 994c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 995c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 996895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 997c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 998895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!mNewData) { 999895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "no new data"); 1000895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 1001c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 1002895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mNewData = 0; 1003895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1004895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* google timestamp */ 1005895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 1006895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (int i = 0; i < numSensors; i++) { 1007895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mEnabled & (1 << i)) { 1008895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i, 1009895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall &mPendingMask, i); 1010895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPendingEvents[i].timestamp = irq_timestamp; 1011c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 1012c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 101342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1014895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (int j = 0; count && mPendingMask && j < numSensors; j++) { 1015895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mPendingMask & (1 << j)) { 1016895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPendingMask &= ~(1 << j); 1017895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mEnabled & (1 << j)) { 1018895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *data++ = mPendingEvents[j]; 1019895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall count--; 1020895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall numEventReceived++; 1021895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1022c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 102342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1024c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 1025c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 1026895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 1027c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall return numEventReceived; 102842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 102942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1030c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrallint MPLSensor::getFd() const 103142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 1032895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("MPLSensor::getFd returning %d", data_fd); 1033895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return data_fd; 103442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 103542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1036c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrallint MPLSensor::getAccelFd() const 103742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 1038895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("MPLSensor::getAccelFd returning %d", accel_fd); 1039c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall return accel_fd; 104042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 104142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1042895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::getTimerFd() const 104342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 1044895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("MPLSensor::getTimerFd returning %d", timer_fd); 1045895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return timer_fd; 1046895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1047895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1048895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::getPowerFd() const 1049895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1050895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int hdl = (int) inv_get_serial_handle(); 1051895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("MPLSensor::getPowerFd returning %d", hdl); 1052895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return hdl; 105342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 105442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 105542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getPollTime() 105642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 105742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return mPollTime; 105842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 105942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 106042331858975144405f95243be8427084ee7d478dJean-Baptiste Querubool MPLSensor::hasPendingEvents() const 106142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 1062895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //if we are using the polling workaround, force the main loop to check for data every time 106342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return (mPollTime != -1); 106442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 106542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1066895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::handlePowerEvent() 106742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 106842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 1069895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mpuirq_data irqd; 1070c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 1071895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int fd = (int) inv_get_serial_handle(); 1072895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall read(fd, &irqd, sizeof(irqd)); 1073c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 1074895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) { 1075895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //going to sleep 1076895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sleepEvent(); 1077895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) { 1078895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //waking up 1079895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall wakeEvent(); 108042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 108142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1082895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(fd, MPU_PM_EVENT_HANDLED, 0); 108342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 108442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1085895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::sleepEvent() 108642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 108742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 1088895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 1089895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mEnabled != 0) { 1090895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mForceSleep = true; 1091895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mOldEnabledMask = mEnabled; 1092895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall setPowerStates(0); 109342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 1094895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 109542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 109642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1097895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::wakeEvent() 109842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 109942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 1100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 1101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mForceSleep) { 1102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall setPowerStates((mOldEnabledMask | mEnabled)); 110342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 1104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mForceSleep = false; 1105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 110642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 1107986d38918eb72f00a67189a942da432df17e504eKevin Powell 1108986d38918eb72f00a67189a942da432df17e504eKevin Powell/** fill in the sensor list based on which sensors are configured. 1109986d38918eb72f00a67189a942da432df17e504eKevin Powell * return the number of configured sensors. 1110986d38918eb72f00a67189a942da432df17e504eKevin Powell * parameter list must point to a memory region of at least 7*sizeof(sensor_t) 1111986d38918eb72f00a67189a942da432df17e504eKevin Powell * parameter len gives the length of the buffer pointed to by list 1112986d38918eb72f00a67189a942da432df17e504eKevin Powell */ 1113986d38918eb72f00a67189a942da432df17e504eKevin Powell 1114986d38918eb72f00a67189a942da432df17e504eKevin Powellint MPLSensor::populateSensorList(struct sensor_t *list, int len) 1115986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1116986d38918eb72f00a67189a942da432df17e504eKevin Powell int numsensors; 1117986d38918eb72f00a67189a942da432df17e504eKevin Powell 1118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(len < 7*sizeof(sensor_t)) { 1119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("sensor list too small, not populating."); 1120986d38918eb72f00a67189a942da432df17e504eKevin Powell return 0; 1121986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1122986d38918eb72f00a67189a942da432df17e504eKevin Powell 1123986d38918eb72f00a67189a942da432df17e504eKevin Powell /* fill in the base values */ 1124986d38918eb72f00a67189a942da432df17e504eKevin Powell memcpy(list, sSensorList, sizeof (struct sensor_t) * 7); 1125986d38918eb72f00a67189a942da432df17e504eKevin Powell 1126986d38918eb72f00a67189a942da432df17e504eKevin Powell /* first add gyro, accel and compass to the list */ 1127986d38918eb72f00a67189a942da432df17e504eKevin Powell 1128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in accel values */ 1129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short accelId = inv_get_accel_id(); 1130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(accelId == 0) 1131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { 1132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Can not get accel id"); 1133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fillAccel(accelId, list); 1135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in compass values */ 1137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short compassId = inv_get_compass_id(); 1138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(compassId == 0) 1139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { 1140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Can not get compass id"); 1141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fillCompass(compassId, list); 1143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in gyro values */ 1145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fillGyro(MPU_NAME, list); 1146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(mNineAxisEnabled) 1148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { 1149986d38918eb72f00a67189a942da432df17e504eKevin Powell numsensors = 7; 1150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* all sensors will be added to the list */ 1151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in orientation values */ 1152986d38918eb72f00a67189a942da432df17e504eKevin Powell fillOrientation(list); 1153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in rotation vector values */ 1155986d38918eb72f00a67189a942da432df17e504eKevin Powell fillRV(list); 1156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in gravity values */ 1158986d38918eb72f00a67189a942da432df17e504eKevin Powell fillGravity(list); 1159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in Linear accel values */ 1161986d38918eb72f00a67189a942da432df17e504eKevin Powell fillLinearAccel(list); 1162986d38918eb72f00a67189a942da432df17e504eKevin Powell } else { 1163986d38918eb72f00a67189a942da432df17e504eKevin Powell /* no 9-axis sensors, zero fill that part of the list */ 1164986d38918eb72f00a67189a942da432df17e504eKevin Powell numsensors = 3; 1165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(list+3, 0, 4*sizeof(struct sensor_t)); 1166986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1167986d38918eb72f00a67189a942da432df17e504eKevin Powell 1168986d38918eb72f00a67189a942da432df17e504eKevin Powell return numsensors; 1169986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1170986d38918eb72f00a67189a942da432df17e504eKevin Powell 1171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::fillAccel(unsigned char accel, struct sensor_t *list) 1172986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (accel) { 1174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_LIS331: 1175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_LIS331_RANGE; 1176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_LIS331_RESOLUTION; 1177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_LIS331_POWER; 1178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1179986d38918eb72f00a67189a942da432df17e504eKevin Powell 1180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_LIS3DH: 1181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_LIS3DH_RANGE; 1182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_LIS3DH_RESOLUTION; 1183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_LIS3DH_POWER; 1184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_KXSD9: 1187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_KXSD9_RANGE; 1188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_KXSD9_RESOLUTION; 1189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_KXSD9_POWER; 1190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_KXTF9: 1193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_KXTF9_RANGE; 1194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_KXTF9_RESOLUTION; 1195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_KXTF9_POWER; 1196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_BMA150: 1199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_BMA150_RANGE; 1200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_BMA150_RESOLUTION; 1201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_BMA150_POWER; 1202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_BMA222: 1205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_BMA222_RANGE; 1206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_BMA222_RESOLUTION; 1207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_BMA222_POWER; 1208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_BMA250: 1211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; 1212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; 1213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_BMA250_POWER; 1214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_ADXL34X: 1217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_ADXL34X_RANGE; 1218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_ADXL34X_RESOLUTION; 1219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_ADXL34X_POWER; 1220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_MMA8450: 1223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_MMA845X: 1229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_MMA845X_RANGE; 1230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_MMA845X_RESOLUTION; 1231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_MMA845X_POWER; 1232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_MPU6050: 1235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE; 1236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION; 1237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_MPU6050_POWER; 1238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 1240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("unknown accel id -- accel params will be wrong."); 1241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1242986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1244986d38918eb72f00a67189a942da432df17e504eKevin Powell 1245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::fillCompass(unsigned char compass, struct sensor_t *list) 1246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (compass) { 1248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_AK8975: 1249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_AKM8975_RANGE; 1250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_AKM8975_RESOLUTION; 1251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_AKM8975_POWER; 1252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_AMI30X: 1254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_AMI30X_RANGE; 1255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_AMI30X_RESOLUTION; 1256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_AMI30X_POWER; 1257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_AMI306: 1259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_AMI306_RANGE; 1260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; 1261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_AMI306_POWER; 1262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_YAS529: 1264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_YAS529_RANGE; 1265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; 1266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_AMI306_POWER; 1267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_YAS530: 1269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_YAS530_RANGE; 1270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION; 1271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_YAS530_POWER; 1272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_HMC5883: 1274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_HMC5883_RANGE; 1275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_HMC5883_RESOLUTION; 1276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_HMC5883_POWER; 1277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_MMC314X: 1279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_MMC314X_RANGE; 1280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_MMC314X_RESOLUTION; 1281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_MMC314X_POWER; 1282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_HSCDTD002B: 1284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_HSCDTD002B_RANGE; 1285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_HSCDTD002B_RESOLUTION; 1286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_HSCDTD002B_POWER; 1287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_HSCDTD004A: 1289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_HSCDTD004A_RANGE; 1290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_HSCDTD004A_RESOLUTION; 1291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_HSCDTD004A_POWER; 1292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 1294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("unknown compass id -- compass parameters will be wrong"); 1295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1296986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1297986d38918eb72f00a67189a942da432df17e504eKevin Powell 1298986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) 1299986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((gyro != NULL) && (strcmp(gyro, "mpu3050") == 0)) { 1301986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].maxRange = GYRO_MPU3050_RANGE; 1302986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; 1303986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].power = GYRO_MPU3050_POWER; 1304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1305986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].maxRange = GYRO_MPU6050_RANGE; 1306986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].resolution = GYRO_MPU6050_RESOLUTION; 1307986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].power = GYRO_MPU6050_POWER; 1308986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1309986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1310986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1311986d38918eb72f00a67189a942da432df17e504eKevin Powell 1312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* fillRV depends on values of accel and compass in the list */ 1314986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillRV(struct sensor_t *list) 1315986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1316986d38918eb72f00a67189a942da432df17e504eKevin Powell /* compute power on the fly */ 1317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[RotationVector].power = list[Gyro].power + list[Accelerometer].power 1318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall + list[MagneticField].power; 1319986d38918eb72f00a67189a942da432df17e504eKevin Powell list[RotationVector].resolution = .00001; 1320986d38918eb72f00a67189a942da432df17e504eKevin Powell list[RotationVector].maxRange = 1.0; 1321986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1322986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1323986d38918eb72f00a67189a942da432df17e504eKevin Powell 1324986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillOrientation(struct sensor_t *list) 1325986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Orientation].power = list[Gyro].power + list[Accelerometer].power 1327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall + list[MagneticField].power; 1328986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Orientation].resolution = .00001; 1329986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Orientation].maxRange = 360.0; 1330986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1331986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1332986d38918eb72f00a67189a942da432df17e504eKevin Powell 1333986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillGravity( struct sensor_t *list) 1334986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Gravity].power = list[Gyro].power + list[Accelerometer].power 1336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall + list[MagneticField].power; 1337986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gravity].resolution = .00001; 1338986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gravity].maxRange = 9.81; 1339986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1340986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1341986d38918eb72f00a67189a942da432df17e504eKevin Powell 1342986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillLinearAccel(struct sensor_t *list) 1343986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Gravity].power = list[Gyro].power + list[Accelerometer].power 1345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall + list[MagneticField].power; 1346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Gravity].resolution = list[Accelerometer].resolution; 1347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Gravity].maxRange = list[Accelerometer].maxRange; 1348986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1349986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1350