MPLSensor.cpp revision 895401859313187f15a800e62d43e6bcbf48fada
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), 145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mMpuAccuracy(0), 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 mPendingEvents[RotationVector].acceleration.status 22442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 22542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 22642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); 22742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].sensor = ID_LA; 22842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; 22942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].acceleration.status 23042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 23142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 23242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].version = sizeof(sensors_event_t); 23342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].sensor = ID_GR; 23442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; 23542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; 23642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 23742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].version = sizeof(sensors_event_t); 23842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].sensor = ID_GY; 23942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; 24042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; 24142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 24242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); 24342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].sensor = ID_A; 24442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; 24542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].acceleration.status 24642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 24742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 24842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].version = sizeof(sensors_event_t); 24942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].sensor = ID_M; 25042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; 251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; 25242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 25342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].version = sizeof(sensors_event_t); 25442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].sensor = ID_O; 25542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; 25642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].orientation.status 25742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 25842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 25942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[RotationVector] = &MPLSensor::rvHandler; 26042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[LinearAccel] = &MPLSensor::laHandler; 26142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Gravity] = &MPLSensor::gravHandler; 26242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Gyro] = &MPLSensor::gyroHandler; 26342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Accelerometer] = &MPLSensor::accelHandler; 26442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[MagneticField] = &MPLSensor::compassHandler; 26542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Orientation] = &MPLSensor::orienHandler; 26642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (int i = 0; i < numSensors; i++) 268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mDelays[i] = 30000000LLU; // 30 ms by default 269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_serial_start(port) != INV_SUCCESS) { 271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal Error : could not open MPL serial interface"); 27242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 27342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //initialize library parameters 275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall initMPL(); 27642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //setup the FIFO contents 278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall setupFIFO(); 27942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //we start the motion processing only when a sensor is enabled... 281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //rv = inv_dmp_start(); 282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGE_IF(rv != INV_SUCCESS, "Fatal error: could not start the DMP correctly. (code = %d)\n", rv); 283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //dmp_started = true; 28442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 28642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 28842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 289895401859313187f15a800e62d43e6bcbf48fadaJP AbgrallMPLSensor::~MPLSensor() 290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_dmp_stop() != INV_SUCCESS) { 294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGW("Error: could not stop the DMP correctly.\n"); 29542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 29642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_dmp_close() != INV_SUCCESS) { 298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGW("Error: could not close the DMP"); 29942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 30042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_serial_stop() != INV_SUCCESS) { 302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGW("Error : could not close the serial port"); 30342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_destroy(&mMplMutex); 30642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 30742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* clear any data from our various filehandles */ 309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::clearIrqData(bool* irq_set) 31042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned int i; 312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int nread; 313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mpuirq_data irqdata; 314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall poll(mPollFds, ARRAY_SIZE(mPollFds), 0); //check which ones need to be cleared 316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < ARRAY_SIZE(mPollFds); i++) { 318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int cur_fd = mPollFds[i].fd; 319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int j = 0; 320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mPollFds[i].revents & POLLIN) { 321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall nread = read(cur_fd, &irqdata, sizeof(irqdata)); 322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (nread > 0) { 323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall irq_set[i] = true; 324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall irq_timestamp = irqdata.irqtime; 325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV_IF(EXTRA_VERBOSE, "irq: %d %d (%d)", i, irqdata.interruptcount, j++); 326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollFds[i].revents = 0; 32942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 33042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 33142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* set the power states of the various sensors based on the bits set in the 333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * enabled_sensors parameter. 334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * this function modifies globalish state variables. It must be called with the mMplMutex held. */ 335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::setPowerStates(int enabled_sensors) 33642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bool irq_set[5] = { false, false, false, false, false }; 33942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV(" setPowerStates: %d dmp_started: %d", enabled_sensors, mDmpStarted); 34142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall do { 34342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { 345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask = ALL_MPL_SENSORS_NP; 346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 34742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 34842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) { 350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask = 0; 351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 35242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 35342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (GY_ENABLED) { 355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask |= INV_THREE_AXIS_GYRO; 356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; 35842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 35942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (A_ENABLED) { 361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask |= (INV_THREE_AXIS_ACCEL); 362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL); 36442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 36542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (M_ENABLED) { 367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask |= INV_THREE_AXIS_COMPASS; 368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS); 370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 37142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } while (0); 37342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //record the new sensor state 375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t rv; 37642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long sen_mask = mLocalSensorMask & mMasterSensorMask; 378c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bool changing_sensors = ((inv_get_dl_config()->requested_sensors 380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall != sen_mask) && (sen_mask != 0)); 381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bool restart = (!mDmpStarted) && (sen_mask != 0); 382c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (changing_sensors || restart) { 384c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart); 386c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mDmpStarted) { 388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_dmp_stop(); 389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clearIrqData(irq_set); 390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mDmpStarted = false; 391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 39242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sen_mask != inv_get_dl_config()->requested_sensors) { 394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("setPowerStates: %lx", sen_mask); 395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = inv_set_mpu_sensors(sen_mask); 396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(rv != INV_SUCCESS, 397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "error: unable to set MPL sensor power states (sens=%ld retcode = %d)", 398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sen_mask, rv); 399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 400c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS)) 402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL))) 403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall && ((sen_mask & INV_DMP_PROCESSOR) == 0)) { 404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ"); 405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mUseTimerirq = true; 406895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mUseTimerirq) { 408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clearIrqData(irq_set); 410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ"); 412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mUseTimerirq = false; 413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 41442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!mDmpStarted) { 416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mHaveGoodMpuCal || mHaveGoodCompassCal) { 417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = inv_store_calibration(); 418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(rv != INV_SUCCESS, 419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "error: unable to store MPL calibration file"); 420895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mHaveGoodMpuCal = false; 421895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mHaveGoodCompassCal = false; 422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("Starting DMP"); 424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = inv_dmp_start(); 425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp"); 426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mDmpStarted = true; 427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 42842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 42942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 430895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //check if we should stop the DMP 431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mDmpStarted && (sen_mask == 0)) { 432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("Stopping DMP"); 433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = inv_dmp_stop(); 434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)", 435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv); 436895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mUseTimerirq) { 437895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 43842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 439895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clearIrqData(irq_set); 440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mDmpStarted = false; 442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPollTime = -1; 443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mCurFifoRate = -1; 44442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 44542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 446c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 44742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 449895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * container function for all the calls we make once to set up the MPL. 450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::initMPL() 452c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall{ 453895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short bias_update_mask = 0xFFFF; 456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg; 45742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_dmp_open() != INV_SUCCESS) { 459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal Error : could not open DMP correctly.\n"); 46042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 46142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work 463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(result != INV_SUCCESS, 464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "Fatal Error : could not set enabled sensors."); 46542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_load_calibration() != INV_SUCCESS) { 467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("could not open MPL calibration file"); 468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //check for the 9axis fusion library: if available load it and start 9x 471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall void* h_dmp_lib=dlopen("libinvensense_mpl.so", RTLD_NOW); 472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(h_dmp_lib) { 473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const char* error; 474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall error = dlerror(); 475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t (*fp_inv_enable_9x_fusion)() = 476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion"); 477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if((error = dlerror()) != NULL) { 478895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("%s %s", error, "inv_enable_9x_fusion"); 479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) { 480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE( "Warning : 9 axis sensor fusion not available " 481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "- No compass detected.\n"); 482895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 483895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* 9axis is loaded and enabled */ 484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* this variable is used for coming up with sensor list */ 485895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mNineAxisEnabled = true; 486c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall const char* error = dlerror(); 489895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error); 49042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 49142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldl_cfg = inv_get_dl_config(); 49342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) { 495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Error : Bias update function could not be set.\n"); 496895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 49742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_motion_interrupt(1) != INV_SUCCESS) { 499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Error : could not set motion interrupt"); 500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 50142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 502895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_fifo_interrupt(1) != INV_SUCCESS) { 503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Error : could not set fifo interrupt"); 50442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 505895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 506895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_fifo_rate(6); 507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result); 50942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 51042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 511895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mMpuAccuracy = SENSOR_STATUS_ACCURACY_MEDIUM; 512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall setupCallbacks(); 51342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 51442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 51542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** setup the fifo contents. 517895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 518895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::setupFIFO() 51942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 52242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 523895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_accel(INV_ALL, INV_32_BIT); 524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_accel returned %d\n", result); 52642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 52742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 528895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_quaternion(INV_32_BIT); 529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_quaternion returned %d\n", result); 531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 532c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_linear_accel(INV_ALL, INV_32_BIT); 534895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_linear_accel returned %d\n", result); 53642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 53742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 538895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_linear_accel_in_world(INV_ALL, INV_32_BIT); 539895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_linear_accel_in_world returned %d\n", 541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result); 542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 54342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 544895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_gravity(INV_ALL, INV_32_BIT); 545895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 546895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_gravity returned %d\n", result); 547c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 54842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_send_gyro(INV_ALL, INV_32_BIT); 550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result != INV_SUCCESS) { 551895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Fatal error: inv_send_gyro returned %d\n", result); 552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 55342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 55442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 55542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 556895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 557895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * set up the callbacks that we use in all cases (outside of gestures, etc) 558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::setupCallbacks() 56042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 561895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 562895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) { 563895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Error : Motion callback could not be set.\n"); 56442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 56542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 56642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) { 568895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Error : Processed data callback could not be set."); 56942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 57042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 571c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 57242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * handle the motion/no motion output from the MPL. 575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::cbOnMotion(uint16_t val) 577c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall{ 578895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //after the first no motion, the gyro should be calibrated well 580895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (val == 2) { 581895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mMpuAccuracy = SENSOR_STATUS_ACCURACY_HIGH; 582895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) { 583895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //if gyros are on and we got a no motion, set a flag 584895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // indicating that the cal file can be written. 585895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mHaveGoodMpuCal = true; 586895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 587895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 58842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 589895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return; 59042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 59142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 592c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 593895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::cbProcData() 594895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 595895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mNewData = 1; 596895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mSampleCount++; 597895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV_IF(EXTRA_VERBOSE, "new data (%d)", sampleCount); 598c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 599c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 600895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall//these handlers transform mpl data into one of the Android sensor types 601895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall// scaling and coordinate transforms should be done in the handlers 602895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 603895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask, 604895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 60542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 60642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 607895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 608895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_GYROS, s->gyro.v); 609895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0; 610895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0; 611895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0; 612895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.status = mMpuAccuracy; 613895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 614895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 615895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 616895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 617895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask, 618895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 619895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 620895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //VFUNC_LOG; 621895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 622895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_ACCELS, s->acceleration.v); 623895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //res = inv_get_accel_float(s->acceleration.v); 624895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->acceleration.v[0] = s->acceleration.v[0] * 9.81; 625895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->acceleration.v[1] = s->acceleration.v[1] * 9.81; 626895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->acceleration.v[2] = s->acceleration.v[2] * 9.81; 627895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV_IF(EXTRA_VERBOSE, "accel data: %f %f %f", s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2]); 628895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; 629895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 630895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 631895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 632895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 633895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::estimateCompassAccuracy() 634895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 635895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 636895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int rv; 637895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 638895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_compass_accuracy(&rv); 639895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(rv >= SENSOR_STATUS_ACCURACY_MEDIUM) { 640895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mHaveGoodCompassCal = true; 641895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 642895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(res != INV_SUCCESS, "error returned from inv_get_compass_accuracy"); 643c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 644895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return rv; 64542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 64642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 647895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask, 648895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 64942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 650c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall VFUNC_LOG; 651895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res, res2; 652895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float bias_error[3]; 653895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float total_be; 654895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static int bias_error_settled = 0; 65542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 656895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v); 65742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 658895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res != INV_SUCCESS) { 659895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGW( 660895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d", 661895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res); 66242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 663895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 664895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->magnetic.status = estimateCompassAccuracy(); 665895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 666895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 667895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 66842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 66942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 670895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask, 671895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 67242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 67342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 674895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float quat[4]; 675895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float norm = 0; 676895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float ang = 0; 677895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t r; 67842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 679895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall r = inv_get_float_array(INV_QUATERNION, quat); 68042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 681895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (r != INV_SUCCESS) { 682895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask &= ~(1 << index); 683895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return; 684c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } else { 685895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 686c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 68742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 688895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3] 689895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall + FLT_EPSILON; 69042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 691895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (norm > 1.0f) { 692895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //renormalize 693895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall norm = sqrtf(norm); 694895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float inv_norm = 1.0f / norm; 695895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[1] = quat[1] * inv_norm; 696895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[2] = quat[2] * inv_norm; 697895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[3] = quat[3] * inv_norm; 698895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 699c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 700895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (quat[0] < 0.0) { 701895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[1] = -quat[1]; 702895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[2] = -quat[2]; 703895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall quat[3] = -quat[3]; 704c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 70542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 706895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[0] = quat[1]; 707895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[1] = quat[2]; 708895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[2] = quat[3]; 709895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 710895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.status 711895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall = ((mMpuAccuracy < estimateCompassAccuracy()) ? mMpuAccuracy 712895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall : estimateCompassAccuracy()); 71342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 71442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 715895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask, 716895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 71742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 718c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall VFUNC_LOG; 719895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 720895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_LINEAR_ACCELERATION, s->gyro.v); 721895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[0] *= 9.81; 722895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[1] *= 9.81; 723895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[2] *= 9.81; 724895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.status = mMpuAccuracy; 725895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 726895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 727895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 728895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 729895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask, 730895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) 731895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 732895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall VFUNC_LOG; 733895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 734895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_GRAVITY, s->gyro.v); 735895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[0] *= 9.81; 736895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[1] *= 9.81; 737895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.v[2] *= 9.81; 738895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->gyro.status = mMpuAccuracy; 739895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 740895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 741895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 74242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 743895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::calcOrientationSensor(float *R, float *values) 744895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 745895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float tmp; 746c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 747895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Azimuth 748895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) { 749895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[0] = (float) atan2f(-R[3], R[0]); 750895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 751895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[0] = (float) atan2f(R[1], R[4]); 752895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 753895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[0] *= 57.295779513082320876798154814105f; 754895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (values[0] < 0) { 755895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[0] += 360.0f; 756895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 757895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Pitch 758895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = R[7]; 759895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp > 1.0f) 760895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = 1.0f; 761895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (tmp < -1.0f) 762895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp = -1.0f; 763895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[1] = -asinf(tmp) * 57.295779513082320876798154814105f; 764895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (R[8] < 0) { 765895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[1] = 180.0f - values[1]; 766895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 767895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (values[1] > 180.0f) { 768895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[1] -= 360.0f; 769895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 770895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Roll 771895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((R[7] > 0.7071067f)) { 772895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[2] = (float) atan2f(R[6], R[7]); 773895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 774895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[2] = (float) atan2f(R[6], R[8]); 77542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 77642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 777895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[2] *= 57.295779513082320876798154814105f; 778895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (values[2] > 90.0f) { 779895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[2] = 180.0f - values[2]; 780895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 781895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (values[2] < -90.0f) { 782895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall values[2] = -180.0f - values[2]; 783c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 78442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 78542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 786895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::orienHandler(sensors_event_t* s, uint32_t* pending_mask, 787895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int index) //note that this is the handler for the android 'orientation' sensor, not the mpl orientation output 78842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 789c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall VFUNC_LOG; 790895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; 791895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float euler[3]; 792895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float heading[1]; 793895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float rot_mat[9]; 794c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 795895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat); 796c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 797895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ComputeAndOrientation(heading[0], euler, s->orientation.v); 798895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall calcOrientationSensor(rot_mat, s->orientation.v); 799c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 800895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall s->orientation.status = estimateCompassAccuracy(); 801c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 802895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (res == INV_SUCCESS) 803895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *pending_mask |= (1 << index); 804895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 805895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGW("orienHandler: data not valid (%d)", (int) res); 806c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 807c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 808c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 809895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::enable(int32_t handle, int en) 810895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 811895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 812895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("handle : %d en: %d", handle, en); 813c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 814895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int what = -1; 815c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 816c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall switch (handle) { 817c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_A: 818c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = Accelerometer; 819c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 820c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_M: 82142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = MagneticField; 82242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 82342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_O: 82442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Orientation; 82542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 82642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GY: 82742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gyro; 82842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 82942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GR: 83042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gravity; 83142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 83242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_RV: 83342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = RotationVector; 83442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 83542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_LA: 83642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = LinearAccel; 83742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 838c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall default: //this takes care of all the gestures 83942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = handle; 84042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 84142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 84242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 84342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (uint32_t(what) >= numSensors) 84442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 84542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 846c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall int newState = en ? 1 : 0; 847c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall int err = 0; 848895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV_IF((uint32_t(newState) << what) != (mEnabled & (1 << what)), 849895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // "sensor state change what=%d", what); 850c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 851895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 852c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { 853c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall uint32_t sensor_type; 854c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall short flags = newState; 855c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall mEnabled &= ~(1 << what); 856c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall mEnabled |= (uint32_t(flags) << what); 857895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "mEnabled = %x", mEnabled); 858895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall setPowerStates(mEnabled); 859895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 860895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!newState) 861895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall update_delay(); 862895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return err; 863895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 864895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 865c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall return err; 866c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 867c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 868c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrallint MPLSensor::setDelay(int32_t handle, int64_t ns) 869c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall{ 870895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 871895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, 872895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall " setDelay handle: %d rate %d", handle, (int) (ns / 1000000LL)); 873c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall int what = -1; 874c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall switch (handle) { 875c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_A: 876c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = Accelerometer; 877c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 878c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_M: 879c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = MagneticField; 880c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 881c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_O: 882c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = Orientation; 883c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 884c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_GY: 885c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = Gyro; 886c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 887c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_GR: 888c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = Gravity; 889c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 890c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_RV: 891c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = RotationVector; 892c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 893c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall case ID_LA: 894c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = LinearAccel; 895c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 896895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 897c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall what = handle; 898c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall break; 899c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 900c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 901c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall if (uint32_t(what) >= numSensors) 902c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall return -EINVAL; 903c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 90442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (ns < 0) 90542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 90642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 907895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 90842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDelays[what] = ns; 909895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 910895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return update_delay(); 91142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 91242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 913895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::update_delay() 914895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 915895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall FUNC_LOG; 916895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int rv = 0; 917895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bool irq_set[5]; 91842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 919895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 92042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 92142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled) { 92242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru uint64_t wanted = -1LLU; 92342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (int i = 0; i < numSensors; i++) { 92442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled & (1 << i)) { 92542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru uint64_t ns = mDelays[i]; 92642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru wanted = wanted < ns ? wanted : ns; 92742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 92842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 92942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 930895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns 931895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (wanted < 10000000LLU) { 932895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall wanted = 10000000LLU; 93342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 93442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 935895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1 936895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall : 0); //mpu fifo rate is in increments of 5ms 937895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (rate == 0) //KLP disallow fifo rate 0 938895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rate = 1; 93942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 940895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (rate != mCurFifoRate) { 941895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGD("set fifo rate: %d %llu", rate, wanted); 942895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t res; // = inv_dmp_stop(); 943895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall res = inv_set_fifo_rate(rate); 944895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate"); 94542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 946895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //res = inv_dmp_start(); 947895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGE_IF(res != INV_SUCCESS, "error re-starting DMP"); 948c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 949895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mCurFifoRate = rate; 950895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = (res == INV_SUCCESS); 951895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 95242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 953895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) { 954895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mUseTimerirq) { 955895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 956895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clearIrqData(irq_set); 957895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_dl_config()->requested_sensors 958895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall == INV_THREE_AXIS_COMPASS) { 959895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, 960895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (unsigned long) (wanted / 1000000LLU)); 961895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", 962895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (int) (wanted / 1000000LLU)); 96342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 964895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, 965895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (unsigned long) inv_get_sample_step_size_ms()); 966895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", 967895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (int) inv_get_sample_step_size_ms()); 96842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 96942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 97042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 97142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 97242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 973895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 974895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return rv; 97542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 97642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 977895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* return the current time in nanoseconds */ 978895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint64_t MPLSensor::now_ns(void) 97942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 980895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //FUNC_LOG; 981895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct timespec ts; 98242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 983895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clock_gettime(CLOCK_MONOTONIC, &ts); 984895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("Time %lld", (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec); 985895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; 986c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall} 987c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 988895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::readEvents(sensors_event_t* data, int count) 989c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall{ 990895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //VFUNC_LOG; 991895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i; 992895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bool irq_set[5] = { false, false, false, false, false }; 993895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t rv; 994895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (count < 1) 995895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return -EINVAL; 996c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall int numEventReceived = 0; 997c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 998895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall clearIrqData(irq_set); 99942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1000895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 1001895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mDmpStarted) { 1002895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV_IF(EXTRA_VERBOSE, "Update Data"); 1003895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall rv = inv_update_data(); 1004895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv); 1005c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 1006c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 1007895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else { 1008895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //probably just one extra read after shutting down 1009895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, 1010895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall "MPLSensor::readEvents called, but there's nothing to do."); 1011c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 1012c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 1013895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 1014c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 1015895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!mNewData) { 1016895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGV_IF(EXTRA_VERBOSE, "no new data"); 1017895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 1018c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 1019895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mNewData = 0; 1020895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1021895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* google timestamp */ 1022895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 1023895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (int i = 0; i < numSensors; i++) { 1024895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mEnabled & (1 << i)) { 1025895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i, 1026895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall &mPendingMask, i); 1027895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPendingEvents[i].timestamp = irq_timestamp; 1028c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 1029c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 103042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1031895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (int j = 0; count && mPendingMask && j < numSensors; j++) { 1032895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mPendingMask & (1 << j)) { 1033895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mPendingMask &= ~(1 << j); 1034895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mEnabled & (1 << j)) { 1035895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *data++ = mPendingEvents[j]; 1036895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall count--; 1037895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall numEventReceived++; 1038895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1039c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 104042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1041c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall } 1042c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 1043895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 1044c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall return numEventReceived; 104542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 104642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1047c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrallint MPLSensor::getFd() const 104842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 1049895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("MPLSensor::getFd returning %d", data_fd); 1050895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return data_fd; 105142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 105242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1053c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrallint MPLSensor::getAccelFd() const 105442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 1055895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("MPLSensor::getAccelFd returning %d", accel_fd); 1056c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall return accel_fd; 105742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 105842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1059895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::getTimerFd() const 106042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 1061895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("MPLSensor::getTimerFd returning %d", timer_fd); 1062895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return timer_fd; 1063895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1064895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1065895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint MPLSensor::getPowerFd() const 1066895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1067895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int hdl = (int) inv_get_serial_handle(); 1068895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //ALOGV("MPLSensor::getPowerFd returning %d", hdl); 1069895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return hdl; 107042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 107142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 107242331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getPollTime() 107342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 107442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return mPollTime; 107542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 107642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 107742331858975144405f95243be8427084ee7d478dJean-Baptiste Querubool MPLSensor::hasPendingEvents() const 107842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 1079895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //if we are using the polling workaround, force the main loop to check for data every time 108042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return (mPollTime != -1); 108142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 108242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1083895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::handlePowerEvent() 108442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 108542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 1086895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mpuirq_data irqd; 1087c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 1088895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int fd = (int) inv_get_serial_handle(); 1089895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall read(fd, &irqd, sizeof(irqd)); 1090c0aca57ba5869c883bb8ce2dd1d90db86c8212f9JP Abgrall 1091895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) { 1092895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //going to sleep 1093895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sleepEvent(); 1094895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) { 1095895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //waking up 1096895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall wakeEvent(); 109742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 109842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1099895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ioctl(fd, MPU_PM_EVENT_HANDLED, 0); 110042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 110142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::sleepEvent() 110342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 110442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 1105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 1106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mEnabled != 0) { 1107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mForceSleep = true; 1108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mOldEnabledMask = mEnabled; 1109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall setPowerStates(0); 111042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 1111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 111242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 111342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::wakeEvent() 111542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 111642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 1117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_lock(&mMplMutex); 1118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mForceSleep) { 1119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall setPowerStates((mOldEnabledMask | mEnabled)); 112042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 1121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mForceSleep = false; 1122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pthread_mutex_unlock(&mMplMutex); 112342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 1124986d38918eb72f00a67189a942da432df17e504eKevin Powell 1125986d38918eb72f00a67189a942da432df17e504eKevin Powell/** fill in the sensor list based on which sensors are configured. 1126986d38918eb72f00a67189a942da432df17e504eKevin Powell * return the number of configured sensors. 1127986d38918eb72f00a67189a942da432df17e504eKevin Powell * parameter list must point to a memory region of at least 7*sizeof(sensor_t) 1128986d38918eb72f00a67189a942da432df17e504eKevin Powell * parameter len gives the length of the buffer pointed to by list 1129986d38918eb72f00a67189a942da432df17e504eKevin Powell */ 1130986d38918eb72f00a67189a942da432df17e504eKevin Powell 1131986d38918eb72f00a67189a942da432df17e504eKevin Powellint MPLSensor::populateSensorList(struct sensor_t *list, int len) 1132986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1133986d38918eb72f00a67189a942da432df17e504eKevin Powell int numsensors; 1134986d38918eb72f00a67189a942da432df17e504eKevin Powell 1135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(len < 7*sizeof(sensor_t)) { 1136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("sensor list too small, not populating."); 1137986d38918eb72f00a67189a942da432df17e504eKevin Powell return 0; 1138986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1139986d38918eb72f00a67189a942da432df17e504eKevin Powell 1140986d38918eb72f00a67189a942da432df17e504eKevin Powell /* fill in the base values */ 1141986d38918eb72f00a67189a942da432df17e504eKevin Powell memcpy(list, sSensorList, sizeof (struct sensor_t) * 7); 1142986d38918eb72f00a67189a942da432df17e504eKevin Powell 1143986d38918eb72f00a67189a942da432df17e504eKevin Powell /* first add gyro, accel and compass to the list */ 1144986d38918eb72f00a67189a942da432df17e504eKevin Powell 1145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in accel values */ 1146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short accelId = inv_get_accel_id(); 1147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(accelId == 0) 1148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { 1149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Can not get accel id"); 1150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fillAccel(accelId, list); 1152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in compass values */ 1154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned short compassId = inv_get_compass_id(); 1155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(compassId == 0) 1156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { 1157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("Can not get compass id"); 1158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fillCompass(compassId, list); 1160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in gyro values */ 1162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fillGyro(MPU_NAME, list); 1163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(mNineAxisEnabled) 1165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { 1166986d38918eb72f00a67189a942da432df17e504eKevin Powell numsensors = 7; 1167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* all sensors will be added to the list */ 1168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in orientation values */ 1169986d38918eb72f00a67189a942da432df17e504eKevin Powell fillOrientation(list); 1170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in rotation vector values */ 1172986d38918eb72f00a67189a942da432df17e504eKevin Powell fillRV(list); 1173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in gravity values */ 1175986d38918eb72f00a67189a942da432df17e504eKevin Powell fillGravity(list); 1176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* fill in Linear accel values */ 1178986d38918eb72f00a67189a942da432df17e504eKevin Powell fillLinearAccel(list); 1179986d38918eb72f00a67189a942da432df17e504eKevin Powell } else { 1180986d38918eb72f00a67189a942da432df17e504eKevin Powell /* no 9-axis sensors, zero fill that part of the list */ 1181986d38918eb72f00a67189a942da432df17e504eKevin Powell numsensors = 3; 1182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(list+3, 0, 4*sizeof(struct sensor_t)); 1183986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1184986d38918eb72f00a67189a942da432df17e504eKevin Powell 1185986d38918eb72f00a67189a942da432df17e504eKevin Powell return numsensors; 1186986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1187986d38918eb72f00a67189a942da432df17e504eKevin Powell 1188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::fillAccel(unsigned char accel, struct sensor_t *list) 1189986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (accel) { 1191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_LIS331: 1192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_LIS331_RANGE; 1193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_LIS331_RESOLUTION; 1194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_LIS331_POWER; 1195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1196986d38918eb72f00a67189a942da432df17e504eKevin Powell 1197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_LIS3DH: 1198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_LIS3DH_RANGE; 1199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_LIS3DH_RESOLUTION; 1200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_LIS3DH_POWER; 1201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_KXSD9: 1204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_KXSD9_RANGE; 1205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_KXSD9_RESOLUTION; 1206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_KXSD9_POWER; 1207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_KXTF9: 1210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_KXTF9_RANGE; 1211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_KXTF9_RESOLUTION; 1212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_KXTF9_POWER; 1213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_BMA150: 1216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_BMA150_RANGE; 1217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_BMA150_RESOLUTION; 1218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_BMA150_POWER; 1219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_BMA222: 1222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_BMA222_RANGE; 1223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_BMA222_RESOLUTION; 1224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_BMA222_POWER; 1225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_BMA250: 1228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; 1229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; 1230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_BMA250_POWER; 1231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_ADXL34X: 1234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_ADXL34X_RANGE; 1235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_ADXL34X_RESOLUTION; 1236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_ADXL34X_POWER; 1237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_MMA8450: 1240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_MMA845X: 1246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_MMA845X_RANGE; 1247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_MMA845X_RESOLUTION; 1248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_MMA845X_POWER; 1249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case ACCEL_ID_MPU6050: 1252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE; 1253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION; 1254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Accelerometer].power = ACCEL_MPU6050_POWER; 1255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 1257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("unknown accel id -- accel params will be wrong."); 1258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1259986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 1261986d38918eb72f00a67189a942da432df17e504eKevin Powell 1262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid MPLSensor::fillCompass(unsigned char compass, struct sensor_t *list) 1263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 1264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (compass) { 1265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_AK8975: 1266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_AKM8975_RANGE; 1267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_AKM8975_RESOLUTION; 1268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_AKM8975_POWER; 1269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_AMI30X: 1271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_AMI30X_RANGE; 1272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_AMI30X_RESOLUTION; 1273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_AMI30X_POWER; 1274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_AMI306: 1276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_AMI306_RANGE; 1277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; 1278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_AMI306_POWER; 1279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_YAS529: 1281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_YAS529_RANGE; 1282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; 1283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_AMI306_POWER; 1284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_YAS530: 1286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_YAS530_RANGE; 1287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION; 1288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_YAS530_POWER; 1289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_HMC5883: 1291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_HMC5883_RANGE; 1292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_HMC5883_RESOLUTION; 1293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_HMC5883_POWER; 1294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_MMC314X: 1296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_MMC314X_RANGE; 1297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_MMC314X_RESOLUTION; 1298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_MMC314X_POWER; 1299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_HSCDTD002B: 1301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_HSCDTD002B_RANGE; 1302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_HSCDTD002B_RESOLUTION; 1303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_HSCDTD002B_POWER; 1304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case COMPASS_ID_HSCDTD004A: 1306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].maxRange = COMPASS_HSCDTD004A_RANGE; 1307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].resolution = COMPASS_HSCDTD004A_RESOLUTION; 1308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[MagneticField].power = COMPASS_HSCDTD004A_POWER; 1309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 1310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 1311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ALOGE("unknown compass id -- compass parameters will be wrong"); 1312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 1313986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1314986d38918eb72f00a67189a942da432df17e504eKevin Powell 1315986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) 1316986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((gyro != NULL) && (strcmp(gyro, "mpu3050") == 0)) { 1318986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].maxRange = GYRO_MPU3050_RANGE; 1319986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; 1320986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].power = GYRO_MPU3050_POWER; 1321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 1322986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].maxRange = GYRO_MPU6050_RANGE; 1323986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].resolution = GYRO_MPU6050_RESOLUTION; 1324986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].power = GYRO_MPU6050_POWER; 1325986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1326986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1327986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1328986d38918eb72f00a67189a942da432df17e504eKevin Powell 1329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* fillRV depends on values of accel and compass in the list */ 1331986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillRV(struct sensor_t *list) 1332986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1333986d38918eb72f00a67189a942da432df17e504eKevin Powell /* compute power on the fly */ 1334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[RotationVector].power = list[Gyro].power + list[Accelerometer].power 1335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall + list[MagneticField].power; 1336986d38918eb72f00a67189a942da432df17e504eKevin Powell list[RotationVector].resolution = .00001; 1337986d38918eb72f00a67189a942da432df17e504eKevin Powell list[RotationVector].maxRange = 1.0; 1338986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1339986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1340986d38918eb72f00a67189a942da432df17e504eKevin Powell 1341986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillOrientation(struct sensor_t *list) 1342986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Orientation].power = list[Gyro].power + list[Accelerometer].power 1344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall + list[MagneticField].power; 1345986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Orientation].resolution = .00001; 1346986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Orientation].maxRange = 360.0; 1347986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1348986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1349986d38918eb72f00a67189a942da432df17e504eKevin Powell 1350986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillGravity( struct sensor_t *list) 1351986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Gravity].power = list[Gyro].power + list[Accelerometer].power 1353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall + list[MagneticField].power; 1354986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gravity].resolution = .00001; 1355986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gravity].maxRange = 9.81; 1356986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1357986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1358986d38918eb72f00a67189a942da432df17e504eKevin Powell 1359986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillLinearAccel(struct sensor_t *list) 1360986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Gravity].power = list[Gyro].power + list[Accelerometer].power 1362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall + list[MagneticField].power; 1363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Gravity].resolution = list[Accelerometer].resolution; 1364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall list[Gravity].maxRange = list[Accelerometer].maxRange; 1365986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1366986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1367986d38918eb72f00a67189a942da432df17e504eKevin Powell 1368