MPLSensor.cpp revision be6c0220038706c329ea7e40bf07b1fa130977c8
142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* 242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * Copyright (C) 2011 Invensense, Inc. 342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * 442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * Licensed under the Apache License, Version 2.0 (the "License"); 542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * you may not use this file except in compliance with the License. 642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * You may obtain a copy of the License at 742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * 842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * http://www.apache.org/licenses/LICENSE-2.0 942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * 1042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * Unless required by applicable law or agreed to in writing, software 1142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * distributed under the License is distributed on an "AS IS" BASIS, 1242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * See the License for the specific language governing permissions and 1442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * limitations under the License. 1542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 1642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 1742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru//#define LOG_NDEBUG 0 1842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru//see also the EXTRA_VERBOSE define, below 1942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 2042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <fcntl.h> 2142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <errno.h> 2242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <math.h> 2342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <float.h> 2442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <poll.h> 2542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <unistd.h> 2642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <dirent.h> 2742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <stdlib.h> 2842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <sys/select.h> 2942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <dlfcn.h> 3042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <pthread.h> 3142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 3242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <cutils/log.h> 3342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include <utils/KeyedVector.h> 3442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 3542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "MPLSensor.h" 3642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 3742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "math.h" 3842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "ml.h" 3942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mlFIFO.h" 4042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mlsl.h" 4142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mlos.h" 4242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "ml_mputest.h" 4342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "ml_stored_data.h" 4442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mldl_cfg.h" 4542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mldl.h" 4642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 4742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mpu.h" 48be6c0220038706c329ea7e40bf07b1fa130977c8Rosa#include "accel.h" 49be6c0220038706c329ea7e40bf07b1fa130977c8Rosa#include "compass.h" 5042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "kernel/timerirq.h" 5142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "kernel/mpuirq.h" 5242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "kernel/slaveirq.h" 5342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 5442331858975144405f95243be8427084ee7d478dJean-Baptiste Queruextern "C" { 5542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mlsupervisor.h" 5642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 5742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 5842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#include "mlcontrol.h" 59986d38918eb72f00a67189a942da432df17e504eKevin Powell#include "sensor_params.h" 6042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 6142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define EXTRA_VERBOSE (0) 6242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__) 6342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define VFUNC_LOG LOGV_IF(EXTRA_VERBOSE, "%s", __PRETTY_FUNCTION__) 6442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* this mask must turn on only the sensors that are present and managed by the MPL */ 6542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_COMPASS | INV_THREE_AXIS_GYRO) 6642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 6742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define CALL_MEMBER_FN(pobject,ptrToMember) ((pobject)->*(ptrToMember)) 6842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 69986d38918eb72f00a67189a942da432df17e504eKevin Powell/******************************************/ 70986d38918eb72f00a67189a942da432df17e504eKevin Powell 71986d38918eb72f00a67189a942da432df17e504eKevin Powell/* Base values for the sensor list, these need to be in the order defined in MPLSensor.h */ 72986d38918eb72f00a67189a942da432df17e504eKevin Powellstatic struct sensor_t sSensorList[] = 73be6c0220038706c329ea7e40bf07b1fa130977c8Rosa { { "MPL Gyroscope", "Invensense", 1, 74986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSORS_GYROSCOPE_HANDLE, 75986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, { } }, 76be6c0220038706c329ea7e40bf07b1fa130977c8Rosa { "MPL Accelerometer", "Invensense", 1, 77986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSORS_ACCELERATION_HANDLE, 78986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, { } }, 79be6c0220038706c329ea7e40bf07b1fa130977c8Rosa { "MPL Magnetic Field", "Invensense", 1, 80986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSORS_MAGNETIC_FIELD_HANDLE, 81986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, { } }, 82be6c0220038706c329ea7e40bf07b1fa130977c8Rosa { "MPL Orientation", "Invensense", 1, 83986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSORS_ORIENTATION_HANDLE, 84986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, { } }, 85be6c0220038706c329ea7e40bf07b1fa130977c8Rosa { "MPL Rotation Vector", "Invensense", 1, 86986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSORS_ROTATION_VECTOR_HANDLE, 87986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, { } }, 88be6c0220038706c329ea7e40bf07b1fa130977c8Rosa { "MPL Linear Acceleration", "Invensense", 1, 89986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSORS_LINEAR_ACCEL_HANDLE, 90986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, { } }, 91be6c0220038706c329ea7e40bf07b1fa130977c8Rosa { "MPL Gravity", "Invensense", 1, 92986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSORS_GRAVITY_HANDLE, 93986d38918eb72f00a67189a942da432df17e504eKevin Powell SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, { } }, 94986d38918eb72f00a67189a942da432df17e504eKevin Powell}; 95986d38918eb72f00a67189a942da432df17e504eKevin Powell 96be6c0220038706c329ea7e40bf07b1fa130977c8Rosastatic unsigned long long irq_timestamp = 0; 9742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* *************************************************************************** 9842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * MPL interface misc. 9942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 10042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru//static pointer to the object that will handle callbacks 10142331858975144405f95243be8427084ee7d478dJean-Baptiste Querustatic MPLSensor* gMPLSensor = NULL; 10242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 10342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* we need to pass some callbacks to the MPL. The mpl is a C library, so 10442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * wrappers for the C++ callback implementations are required. 10542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 10642331858975144405f95243be8427084ee7d478dJean-Baptiste Queruextern "C" { 10742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru//callback wrappers go here 10842331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid mot_cb_wrapper(uint16_t val) 10942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 11042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (gMPLSensor) { 11142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru gMPLSensor->cbOnMotion(val); 11242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 11342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 11442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 11542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid procData_cb_wrapper() 11642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 11742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if(gMPLSensor) { 11842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru gMPLSensor->cbProcData(); 11942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 12042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 12142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 12242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} //end of extern C 12342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 12442331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid setCallbackObject(MPLSensor* gbpt) 12542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 12642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru gMPLSensor = gbpt; 12742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 12842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 12942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 13042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/***************************************************************************** 13142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * sensor class implementation 13242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 13342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 13442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define GY_ENABLED ((1<<ID_GY) & enabled_sensors) 13542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define A_ENABLED ((1<<ID_A) & enabled_sensors) 13642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define O_ENABLED ((1<<ID_O) & enabled_sensors) 13742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define M_ENABLED ((1<<ID_M) & enabled_sensors) 13842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define LA_ENABLED ((1<<ID_LA) & enabled_sensors) 13942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define GR_ENABLED ((1<<ID_GR) & enabled_sensors) 14042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru#define RV_ENABLED ((1<<ID_RV) & enabled_sensors) 14142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 14242331858975144405f95243be8427084ee7d478dJean-Baptiste QueruMPLSensor::MPLSensor() : 143be6c0220038706c329ea7e40bf07b1fa130977c8Rosa SensorBase(NULL, NULL), 144be6c0220038706c329ea7e40bf07b1fa130977c8Rosa mMpuAccuracy(0), mNewData(0), 145be6c0220038706c329ea7e40bf07b1fa130977c8Rosa mDmpStarted(false), 14642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mMasterSensorMask(INV_ALL_SENSORS), 14742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1), 14842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mCurFifoRate(-1), mHaveGoodMpuCal(false), 14942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mUseTimerIrqAccel(false), mUsetimerIrqCompass(true), 150be6c0220038706c329ea7e40bf07b1fa130977c8Rosa mUseTimerirq(false), mSampleCount(0), 151be6c0220038706c329ea7e40bf07b1fa130977c8Rosa mEnabled(0), mPendingMask(0) 15242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 15342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 15442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t rv; 15542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int mpu_int_fd, i; 15642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru char *port = NULL; 15742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 15842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors); 15942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 160be6c0220038706c329ea7e40bf07b1fa130977c8Rosa pthread_mutex_init(&mMplMutex, NULL); 161be6c0220038706c329ea7e40bf07b1fa130977c8Rosa 16242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mForceSleep = false; 16342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 164986d38918eb72f00a67189a942da432df17e504eKevin Powell /* used for identifying whether 9axis is enabled or not */ 165986d38918eb72f00a67189a942da432df17e504eKevin Powell /* this variable will be changed in initMPL() when libmpl is loaded */ 166986d38918eb72f00a67189a942da432df17e504eKevin Powell /* sensor list will be changed based on this variable */ 167986d38918eb72f00a67189a942da432df17e504eKevin Powell mNineAxisEnabled = false; 168986d38918eb72f00a67189a942da432df17e504eKevin Powell 16942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (i = 0; i < ARRAY_SIZE(mPollFds); i++) { 17042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[i].fd = -1; 17142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[i].events = 0; 17242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 17342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 17442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 17542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 17642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mpu_int_fd = open("/dev/mpuirq", O_RDWR); 17742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mpu_int_fd == -1) { 17842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("could not open the mpu irq device node"); 17942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 18042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK); 18142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //ioctl(mpu_int_fd, MPUIRQ_SET_TIMEOUT, 0); 18242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mIrqFds.add(MPUIRQ_FD, mpu_int_fd); 18342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[MPUIRQ_FD].fd = mpu_int_fd; 18442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[MPUIRQ_FD].events = POLLIN; 18542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 18642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 18742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru accel_fd = open("/dev/accelirq", O_RDWR); 18842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (accel_fd == -1) { 18942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("could not open the accel irq device node"); 19042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 19142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru fcntl(accel_fd, F_SETFL, O_NONBLOCK); 19242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //ioctl(accel_fd, SLAVEIRQ_SET_TIMEOUT, 0); 19342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mIrqFds.add(ACCELIRQ_FD, accel_fd); 19442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[ACCELIRQ_FD].fd = accel_fd; 19542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[ACCELIRQ_FD].events = POLLIN; 19642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 19742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 19842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru timer_fd = open("/dev/timerirq", O_RDWR); 19942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (timer_fd == -1) { 20042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("could not open the timer irq device node"); 20142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 20242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru fcntl(timer_fd, F_SETFL, O_NONBLOCK); 20342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //ioctl(timer_fd, TIMERIRQ_SET_TIMEOUT, 0); 20442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mIrqFds.add(TIMERIRQ_FD, timer_fd); 20542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[TIMERIRQ_FD].fd = timer_fd; 20642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[TIMERIRQ_FD].events = POLLIN; 20742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 20842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 20942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru data_fd = mpu_int_fd; 21042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 21142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if ((accel_fd == -1) && (timer_fd != -1)) { 21242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //no accel irq and timer available 21342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mUseTimerIrqAccel = true; 214be6c0220038706c329ea7e40bf07b1fa130977c8Rosa //LOGD("MPLSensor falling back to timerirq for accel data"); 21542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 21642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 21742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru memset(mPendingEvents, 0, sizeof(mPendingEvents)); 21842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 21942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[RotationVector].version = sizeof(sensors_event_t); 22042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[RotationVector].sensor = ID_RV; 22142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; 22242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[RotationVector].acceleration.status 22342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 22442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 22542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); 22642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].sensor = ID_LA; 22742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; 22842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[LinearAccel].acceleration.status 22942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 23042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 23142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].version = sizeof(sensors_event_t); 23242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].sensor = ID_GR; 23342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; 23442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; 23542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 23642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].version = sizeof(sensors_event_t); 23742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].sensor = ID_GY; 23842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; 23942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; 24042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 24142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); 24242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].sensor = ID_A; 24342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; 24442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Accelerometer].acceleration.status 24542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 24642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 24742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].version = sizeof(sensors_event_t); 24842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].sensor = ID_M; 24942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; 25042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; 25142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 25242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].version = sizeof(sensors_event_t); 25342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].sensor = ID_O; 25442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; 25542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingEvents[Orientation].orientation.status 25642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = SENSOR_STATUS_ACCURACY_HIGH; 25742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 25842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[RotationVector] = &MPLSensor::rvHandler; 25942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[LinearAccel] = &MPLSensor::laHandler; 26042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Gravity] = &MPLSensor::gravHandler; 26142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Gyro] = &MPLSensor::gyroHandler; 26242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Accelerometer] = &MPLSensor::accelHandler; 26342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[MagneticField] = &MPLSensor::compassHandler; 26442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHandlers[Orientation] = &MPLSensor::orienHandler; 26542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 26642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (int i = 0; i < numSensors; i++) 26742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDelays[i] = 30000000LLU; // 30 ms by default 26842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 26942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_serial_start(port) != INV_SUCCESS) { 27042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal Error : could not open MPL serial interface"); 27142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 27242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 27342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //initialize library parameters 27442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru initMPL(); 27542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 27642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //setup the FIFO contents 27742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru setupFIFO(); 27842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 27942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //we start the motion processing only when a sensor is enabled... 28042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //rv = inv_dmp_start(); 28142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGE_IF(rv != INV_SUCCESS, "Fatal error: could not start the DMP correctly. (code = %d)\n", rv); 28242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //dmp_started = true; 28342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 28442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 28542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 28642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 28742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 28842331858975144405f95243be8427084ee7d478dJean-Baptiste QueruMPLSensor::~MPLSensor() 28942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 29042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 29142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 29242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_dmp_stop() != INV_SUCCESS) { 29342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGD("Error: could not stop the DMP correctly.\n"); 29442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 29542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 29642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_dmp_close() != INV_SUCCESS) { 29742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGD("Error: could not close the DMP"); 29842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 29942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 30042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_serial_stop() != INV_SUCCESS) { 30142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGD("Error : could not close the serial port"); 30242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 30342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 304be6c0220038706c329ea7e40bf07b1fa130977c8Rosa pthread_mutex_destroy(&mMplMutex); 30542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 30642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 30742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* clear any data from our various filehandles */ 30842331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::clearIrqData(bool* irq_set) 30942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 31042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru unsigned int i; 31142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int nread; 31242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru struct mpuirq_data irqdata; 31342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 31442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru poll(mPollFds, ARRAY_SIZE(mPollFds), 0); //check which ones need to be cleared 31542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 31642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (i = 0; i < ARRAY_SIZE(mPollFds); i++) { 31742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int cur_fd = mPollFds[i].fd; 31842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int j = 0; 31942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mPollFds[i].revents & POLLIN) { 32042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru nread = read(cur_fd, &irqdata, sizeof(irqdata)); 32142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (nread > 0) { 32242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru irq_set[i] = true; 323be6c0220038706c329ea7e40bf07b1fa130977c8Rosa irq_timestamp = irqdata.irqtime; 32442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGV_IF(EXTRA_VERBOSE, "irq: %d %d (%d)", i, irqdata.interruptcount, j++); 32542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 32642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 32742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollFds[i].revents = 0; 32842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 32942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 33042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 33142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* set the power states of the various sensors based on the bits set in the 33242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * enabled_sensors parameter. 33342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * this function modifies globalish state variables. It must be called with the mMplMutex held. */ 33442331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::setPowerStates(int enabled_sensors) 33542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 33642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 33742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru bool irq_set[5] = { false, false, false, false, false }; 33842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 33942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV(" enabled_sensors: %d dmp_started: %d", enabled_sensors, mDmpStarted); 34042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 34142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru do { 34242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 34342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { 34442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask = ALL_MPL_SENSORS_NP; 34542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 34642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 34742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 34842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) { 34942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask = 0; 35042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 35142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 35242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 35342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (GY_ENABLED) { 35442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask |= INV_THREE_AXIS_GYRO; 35542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 35642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; 35742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 35842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 35942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (A_ENABLED) { 36042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask |= (INV_THREE_AXIS_ACCEL); 36142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 36242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL); 36342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 36442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 36542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (M_ENABLED) { 36642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask |= INV_THREE_AXIS_COMPASS; 36742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 36842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS); 36942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 37042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 37142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } while (0); 37242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 37342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //record the new sensor state 37442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t rv; 37542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 37642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru long sen_mask = mLocalSensorMask & mMasterSensorMask; 37742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 37842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru bool changing_sensors = ((inv_get_dl_config()->requested_sensors 37942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru != sen_mask) && (sen_mask != 0)); 38042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru bool restart = (!mDmpStarted) && (sen_mask != 0); 38142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 38242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (changing_sensors || restart) { 38342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 38442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart); 38542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 38642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mDmpStarted) { 38742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_dmp_stop(); 38842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clearIrqData(irq_set); 38942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDmpStarted = false; 39042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 39142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 39242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (sen_mask != inv_get_dl_config()->requested_sensors) { 39342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("inv_set_mpu_sensors: %lx", sen_mask); 39442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv = inv_set_mpu_sensors(sen_mask); 395b60be6e033888799646853aa3284d52809c5d0e0Kevin Powell LOGE_IF(rv != INV_SUCCESS, 39642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "error: unable to set MPL sensor power states (sens=%ld retcode = %d)", 39742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru sen_mask, rv); 39842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 39942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 40042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS)) 40142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL))) 40242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru && ((sen_mask & INV_DMP_PROCESSOR) == 0)) { 40342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ"); 40442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mUseTimerirq = true; 40542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 40642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mUseTimerirq) { 40742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 40842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clearIrqData(irq_set); 40942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 41042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ"); 41142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mUseTimerirq = false; 41242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 41342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 41442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (!mDmpStarted) { 415147e22fb3558b13baed2b0fcf0b5a25f8bcf540bKevin Powell if (mHaveGoodMpuCal) { 416147e22fb3558b13baed2b0fcf0b5a25f8bcf540bKevin Powell rv = inv_store_calibration(); 417147e22fb3558b13baed2b0fcf0b5a25f8bcf540bKevin Powell LOGE_IF(rv != INV_SUCCESS, 418147e22fb3558b13baed2b0fcf0b5a25f8bcf540bKevin Powell "error: unable to store MPL calibration file"); 419147e22fb3558b13baed2b0fcf0b5a25f8bcf540bKevin Powell mHaveGoodMpuCal = false; 420147e22fb3558b13baed2b0fcf0b5a25f8bcf540bKevin Powell } 42142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("Starting DMP"); 42242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv = inv_dmp_start(); 42342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(rv != INV_SUCCESS, "unable to start dmp"); 42442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDmpStarted = true; 42542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 42642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 42742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 42842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //check if we should stop the DMP 42942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mDmpStarted && (sen_mask == 0)) { 43042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("Stopping DMP"); 43142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv = inv_dmp_stop(); 43242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)", 43342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv); 43442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mUseTimerirq) { 43542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 43642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 43742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clearIrqData(irq_set); 43842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 43942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDmpStarted = false; 44042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPollTime = -1; 44142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mCurFifoRate = -1; 44242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 44342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 44442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 44542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 44642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/** 44742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * container function for all the calls we make once to set up the MPL. 44842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 44942331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::initMPL() 45042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 45142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 45242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t result; 45342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru unsigned short bias_update_mask = 0xFFFF; 45442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru struct mldl_cfg *mldl_cfg; 45542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 45642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_dmp_open() != INV_SUCCESS) { 45742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal Error : could not open DMP correctly.\n"); 45842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 45942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 46042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work 46142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(result != INV_SUCCESS, 46242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "Fatal Error : could not set enabled sensors."); 46342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 46442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_load_calibration() != INV_SUCCESS) { 46542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("could not open MPL calibration file"); 46642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 46742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 46842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //check for the 9axis fusion library: if available load it and start 9x 469f55aa1e88c3ca0939be9be8970252927b0cd06eeKevin Powell void* h_dmp_lib=dlopen("libinvensense_mpl.so", RTLD_NOW); 47042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if(h_dmp_lib) { 47142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru const char* error; 472f6d9416a248b91ab93e4de92037dfd585504f4a6Kevin Powell error = dlerror(); 47342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t (*fp_inv_enable_9x_fusion)() = 47442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion"); 47542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if((error = dlerror()) != NULL) { 476f6d9416a248b91ab93e4de92037dfd585504f4a6Kevin Powell LOGE("%s %s", error, "inv_enable_9x_fusion"); 47742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) { 47842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE( "Warning : 9 axis sensor fusion not available " 47942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "- No compass detected.\n"); 480986d38918eb72f00a67189a942da432df17e504eKevin Powell } else { 481986d38918eb72f00a67189a942da432df17e504eKevin Powell /* 9axis is loaded and enabled */ 482986d38918eb72f00a67189a942da432df17e504eKevin Powell /* this variable is used for coming up with sensor list */ 483986d38918eb72f00a67189a942da432df17e504eKevin Powell mNineAxisEnabled = true; 48442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 48542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 48642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru const char* error = dlerror(); 487986d38918eb72f00a67189a942da432df17e504eKevin Powell LOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error); 48842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 48942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 49042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mldl_cfg = inv_get_dl_config(); 49142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 49242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) { 49342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Error : Bias update function could not be set.\n"); 49442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 49542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 49642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_set_motion_interrupt(1) != INV_SUCCESS) { 49742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Error : could not set motion interrupt"); 49842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 49942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 50042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_set_fifo_interrupt(1) != INV_SUCCESS) { 50142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Error : could not set fifo interrupt"); 50242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 50342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 50442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_set_fifo_rate(6); 50542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 50642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_set_fifo_rate returned %d\n", result); 50742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 50842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 50942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mMpuAccuracy = SENSOR_STATUS_ACCURACY_MEDIUM; 51042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru setupCallbacks(); 51142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 51242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 51342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 51442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/** setup the fifo contents. 51542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 51642331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::setupFIFO() 51742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 51842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 51942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t result; 52042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 52142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_accel(INV_ALL, INV_32_BIT); 52242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 52342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_accel returned %d\n", result); 52442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 52542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 52642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_quaternion(INV_32_BIT); 52742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 52842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_quaternion returned %d\n", result); 52942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 53042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 53142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_linear_accel(INV_ALL, INV_32_BIT); 53242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 53342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_linear_accel returned %d\n", result); 53442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 53542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 53642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_linear_accel_in_world(INV_ALL, INV_32_BIT); 53742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 53842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_linear_accel_in_world returned %d\n", 53942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result); 54042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 54142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 54242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_gravity(INV_ALL, INV_32_BIT); 54342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 54442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_gravity returned %d\n", result); 54542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 54642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 54742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru result = inv_send_gyro(INV_ALL, INV_32_BIT); 54842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (result != INV_SUCCESS) { 54942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Fatal error: inv_send_gyro returned %d\n", result); 55042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 55142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 55242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 55342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 55442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/** 55542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * set up the callbacks that we use in all cases (outside of gestures, etc) 55642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 55742331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::setupCallbacks() 55842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 55942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 56042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) { 56142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Error : Motion callback could not be set.\n"); 56242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 56342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 56442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 56542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) { 56642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE("Error : Processed data callback could not be set."); 56742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 56842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 56942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 57042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 57142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/** 57242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru * handle the motion/no motion output from the MPL. 57342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru */ 57442331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::cbOnMotion(uint16_t val) 57542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 57642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 57742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //after the first no motion, the gyro should be calibrated well 57842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (val == 2) { 57942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mMpuAccuracy = SENSOR_STATUS_ACCURACY_HIGH; 580147e22fb3558b13baed2b0fcf0b5a25f8bcf540bKevin Powell if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) { 58142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //if gyros are on and we got a no motion, set a flag 58242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru // indicating that the cal file can be written. 58342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mHaveGoodMpuCal = true; 58442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 58542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 58642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 58742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return; 58842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 58942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 59042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 59142331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::cbProcData() 59242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 59342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mNewData = 1; 59442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mSampleCount++; 59542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGV_IF(EXTRA_VERBOSE, "new data (%d)", sampleCount); 59642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 59742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 59842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru//these handlers transform mpl data into one of the Android sensor types 59942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru// scaling and coordinate transforms should be done in the handlers 60042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 60142331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask, 60242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 60342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 60442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 60542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 60642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_GYROS, s->gyro.v); 60742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0; 60842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0; 60942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0; 61042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.status = mMpuAccuracy; 61142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 61242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 61342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 61442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 61542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask, 61642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 61742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 61842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //VFUNC_LOG; 61942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 62042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_ACCELS, s->acceleration.v); 62142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //res = inv_get_accel_float(s->acceleration.v); 62242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->acceleration.v[0] = s->acceleration.v[0] * 9.81; 62342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->acceleration.v[1] = s->acceleration.v[1] * 9.81; 62442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->acceleration.v[2] = s->acceleration.v[2] * 9.81; 62542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGV_IF(EXTRA_VERBOSE, "accel data: %f %f %f", s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2]); 62642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; 62742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 62842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 62942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 63042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 63142331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::estimateCompassAccuracy() 63242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 63342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 63442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int rv; 63542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 63642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_compass_accuracy(&rv); 63742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(res != INV_SUCCESS, "error returned from inv_get_compass_accuracy"); 63842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 63942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return rv; 64042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 64142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 64242331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask, 64342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 64442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 64542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 64642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res, res2; 64742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float bias_error[3]; 64842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float total_be; 64942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru static int bias_error_settled = 0; 65042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 65142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v); 65242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 65342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res != INV_SUCCESS) { 65442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGD( 65542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d", 65642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res); 65742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 65842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 65942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->magnetic.status = estimateCompassAccuracy(); 66042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 66142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 66242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 66342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 66442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 66542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask, 66642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 66742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 66842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 66942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float quat[4]; 67042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float norm = 0; 67142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float ang = 0; 67242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t r; 67342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 67442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru r = inv_get_float_array(INV_QUATERNION, quat); 67542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 67642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (r != INV_SUCCESS) { 67742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask &= ~(1 << index); 67842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return; 67942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 68042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 68142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 68242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 68342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3] 68442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru + FLT_EPSILON; 68542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 68642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (norm > 1.0f) { 68742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //renormalize 68842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru norm = sqrtf(norm); 68942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float inv_norm = 1.0f / norm; 69042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[1] = quat[1] * inv_norm; 69142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[2] = quat[2] * inv_norm; 69242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[3] = quat[3] * inv_norm; 69342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 69442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 69542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (quat[0] < 0.0) { 69642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[1] = -quat[1]; 69742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[2] = -quat[2]; 69842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru quat[3] = -quat[3]; 69942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 70042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 70142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[0] = quat[1]; 70242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[1] = quat[2]; 70342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[2] = quat[3]; 70442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 70542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.status 70642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = ((mMpuAccuracy < estimateCompassAccuracy()) ? mMpuAccuracy 70742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru : estimateCompassAccuracy()); 70842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 70942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 71042331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask, 71142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 71242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 71342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 71442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 71542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_LINEAR_ACCELERATION, s->gyro.v); 71642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[0] *= 9.81; 71742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[1] *= 9.81; 71842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[2] *= 9.81; 71942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.status = mMpuAccuracy; 72042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 72142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 72242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 72342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 72442331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask, 72542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) 72642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 72742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 72842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 72942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_GRAVITY, s->gyro.v); 73042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[0] *= 9.81; 73142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[1] *= 9.81; 73242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.v[2] *= 9.81; 73342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->gyro.status = mMpuAccuracy; 73442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 73542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 73642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 73742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 73842331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::calcOrientationSensor(float *R, float *values) 73942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 74042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float tmp; 74142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 74242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //Azimuth 74342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) { 74442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[0] = (float) atan2f(-R[3], R[0]); 74542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 74642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[0] = (float) atan2f(R[1], R[4]); 74742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 74842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[0] *= 57.295779513082320876798154814105f; 74942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (values[0] < 0) { 75042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[0] += 360.0f; 75142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 75242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //Pitch 75342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru tmp = R[7]; 75442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (tmp > 1.0f) 75542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru tmp = 1.0f; 75642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (tmp < -1.0f) 75742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru tmp = -1.0f; 75842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[1] = -asinf(tmp) * 57.295779513082320876798154814105f; 75942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (R[8] < 0) { 76042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[1] = 180.0f - values[1]; 76142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 76242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (values[1] > 180.0f) { 76342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[1] -= 360.0f; 76442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 76542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //Roll 76642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if ((R[7] > 0.7071067f)) { 76742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[2] = (float) atan2f(R[6], R[7]); 76842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 76942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[2] = (float) atan2f(R[6], R[8]); 77042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 77142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 77242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[2] *= 57.295779513082320876798154814105f; 77342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (values[2] > 90.0f) { 77442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[2] = 180.0f - values[2]; 77542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 77642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (values[2] < -90.0f) { 77742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru values[2] = -180.0f - values[2]; 77842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 77942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 78042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 78142331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::orienHandler(sensors_event_t* s, uint32_t* pending_mask, 78242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int index) //note that this is the handler for the android 'orientation' sensor, not the mpl orientation output 78342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 78442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 78542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; 78642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float euler[3]; 78742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float heading[1]; 78842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru float rot_mat[9]; 78942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 79042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat); 79142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 79242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //ComputeAndOrientation(heading[0], euler, s->orientation.v); 79342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru calcOrientationSensor(rot_mat, s->orientation.v); 79442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 79542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru s->orientation.status 79642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru = ((mMpuAccuracy < estimateCompassAccuracy()) ? mMpuAccuracy 79742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru : estimateCompassAccuracy()); 79842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 79942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (res == INV_SUCCESS) 80042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *pending_mask |= (1 << index); 80142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru else 80242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGD("orien_handler: data not valid (%d)", (int) res); 80342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 80442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 80542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 80642331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::enable(int32_t handle, int en) 80742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 80842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 80942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("handle : %d en: %d", handle, en); 81042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 81142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int what = -1; 81242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 81342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru switch (handle) { 81442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_A: 81542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Accelerometer; 81642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 81742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_M: 81842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = MagneticField; 81942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 82042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_O: 82142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Orientation; 82242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 82342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GY: 82442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gyro; 82542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 82642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GR: 82742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gravity; 82842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 82942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_RV: 83042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = RotationVector; 83142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 83242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_LA: 83342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = LinearAccel; 83442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 83542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru default: //this takes care of all the gestures 83642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = handle; 83742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 83842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 83942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 84042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (uint32_t(what) >= numSensors) 84142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 84242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 84342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int newState = en ? 1 : 0; 84442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int err = 0; 84542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF((uint32_t(newState) << what) != (mEnabled & (1 << what)), 84642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "sensor state change what=%d", what); 84742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 84842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 84942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { 85042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru uint32_t sensor_type; 85142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru short flags = newState; 85242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mEnabled &= ~(1 << what); 85342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mEnabled |= (uint32_t(flags) << what); 85442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "mEnabled = %x", mEnabled); 85542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru setPowerStates(mEnabled); 85642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 85742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (!newState) 85842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru update_delay(); 85942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return err; 86042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 86142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 86242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return err; 86342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 86442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 86542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::setDelay(int32_t handle, int64_t ns) 86642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 86742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 86842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru /* LOGV_IF(EXTRA_VERBOSE, */ 869be6c0220038706c329ea7e40bf07b1fa130977c8Rosa LOGD(" setDelay handle: %d rate %d", handle, (int) (ns / 1000000LL)); 87042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int what = -1; 87142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru switch (handle) { 87242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_A: 87342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Accelerometer; 87442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 87542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_M: 87642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = MagneticField; 87742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 87842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_O: 87942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Orientation; 88042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 88142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GY: 88242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gyro; 88342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 88442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_GR: 88542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = Gravity; 88642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 88742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_RV: 88842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = RotationVector; 88942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 89042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru case ID_LA: 89142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = LinearAccel; 89242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 89342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru default: 89442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru what = handle; 89542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru break; 89642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 89742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 89842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (uint32_t(what) >= numSensors) 89942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 90042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 90142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (ns < 0) 90242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 90342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 90442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 90542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mDelays[what] = ns; 90642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 90742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return update_delay(); 90842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 90942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 91042331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::update_delay() 91142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 91242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru FUNC_LOG; 91342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int rv = 0; 91442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru bool irq_set[5]; 91542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 91642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 91742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 91842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled) { 91942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru uint64_t wanted = -1LLU; 92042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (int i = 0; i < numSensors; i++) { 92142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled & (1 << i)) { 92242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru uint64_t ns = mDelays[i]; 92342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru wanted = wanted < ns ? wanted : ns; 92442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 92542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 92642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 92742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns 92842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (wanted < 10000000LLU) { 92942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru wanted = 10000000LLU; 93042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 93142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 93242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1 93342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru : 0); //mpu fifo rate is in increments of 5ms 93442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (rate == 0) //KLP disallow fifo rate 0 93542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rate = 1; 93642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 93742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (rate != mCurFifoRate) { 938be6c0220038706c329ea7e40bf07b1fa130977c8Rosa LOGD("set fifo rate: %d %llu", rate, wanted); 93942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t res; // = inv_dmp_stop(); 94042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru res = inv_set_fifo_rate(rate); 94142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(res != INV_SUCCESS, "error setting FIFO rate"); 94242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 94342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //res = inv_dmp_start(); 94442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGE_IF(res != INV_SUCCESS, "error re-starting DMP"); 94542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 94642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mCurFifoRate = rate; 94742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv = (res == INV_SUCCESS); 94842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 94942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 95042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) { 95142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mUseTimerirq) { 95242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); 95342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clearIrqData(irq_set); 95442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (inv_get_dl_config()->requested_sensors 95542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru == INV_THREE_AXIS_COMPASS) { 95642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, 95742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru (unsigned long) (wanted / 1000000LLU)); 95842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", 95942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru (int) (wanted / 1000000LLU)); 96042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else { 96142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, 96242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru (unsigned long) inv_get_sample_step_size_ms()); 96342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", 96442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru (int) inv_get_sample_step_size_ms()); 96542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 96642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 96742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 96842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 96942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 97042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 97142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return rv; 97242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 97342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 97442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru/* return the current time in nanoseconds */ 97542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint64_t MPLSensor::now_ns(void) 97642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 97742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //FUNC_LOG; 97842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru struct timespec ts; 97942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 98042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clock_gettime(CLOCK_MONOTONIC, &ts); 98142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGV("Time %lld", (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec); 98242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; 98342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 98442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 98542331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::readEvents(sensors_event_t* data, int count) 98642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 98742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //VFUNC_LOG; 98842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int i; 98942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru bool irq_set[5] = { false, false, false, false, false }; 99042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru inv_error_t rv; 99142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (count < 1) 99242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return -EINVAL; 99342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int numEventReceived = 0; 99442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 99542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru clearIrqData(irq_set); 99642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 99742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 99842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mDmpStarted) { 99942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //LOGV_IF(EXTRA_VERBOSE, "Update Data"); 100042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru rv = inv_update_data(); 100142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv); 100242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 100342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 100442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru else { 100542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //probably just one extra read after shutting down 100642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, 100742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru "MPLSensor::readEvents called, but there's nothing to do."); 100842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 100942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 101042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 101142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 101242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (!mNewData) { 101342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV_IF(EXTRA_VERBOSE, "no new data"); 101442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return 0; 101542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 101642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mNewData = 0; 1017be6c0220038706c329ea7e40bf07b1fa130977c8Rosa 1018be6c0220038706c329ea7e40bf07b1fa130977c8Rosa /* google timestamp */ 101942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 102042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (int i = 0; i < numSensors; i++) { 102142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled & (1 << i)) { 102242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i, 102342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru &mPendingMask, i); 1024be6c0220038706c329ea7e40bf07b1fa130977c8Rosa mPendingEvents[i].timestamp = irq_timestamp; 102542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 102642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 102742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 102842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru for (int j = 0; count && mPendingMask && j < numSensors; j++) { 102942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mPendingMask & (1 << j)) { 103042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mPendingMask &= ~(1 << j); 103142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled & (1 << j)) { 103242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru *data++ = mPendingEvents[j]; 103342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru count--; 103442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru numEventReceived++; 103542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 103642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 103742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 103842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 103942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 104042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 104142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return numEventReceived; 104242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 104342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 104442331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getFd() const 104542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 104642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("MPLSensor::getFd returning %d", data_fd); 104742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return data_fd; 104842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 104942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 105042331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getAccelFd() const 105142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 105242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("MPLSensor::getAccelFd returning %d", accel_fd); 105342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return accel_fd; 105442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 105542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 105642331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getTimerFd() const 105742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 105842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("MPLSensor::getTimerFd returning %d", timer_fd); 105942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return timer_fd; 106042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 106142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 106242331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getPowerFd() const 106342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 106442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int hdl = (int) inv_get_serial_handle(); 106542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru LOGV("MPLSensor::getPowerFd returning %d", hdl); 106642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return hdl; 106742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 106842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 106942331858975144405f95243be8427084ee7d478dJean-Baptiste Queruint MPLSensor::getPollTime() 107042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 107142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return mPollTime; 107242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 107342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 107442331858975144405f95243be8427084ee7d478dJean-Baptiste Querubool MPLSensor::hasPendingEvents() const 107542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 107642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //if we are using the polling workaround, force the main loop to check for data every time 107742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru return (mPollTime != -1); 107842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 107942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 108042331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::handlePowerEvent() 108142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 108242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 108342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mpuirq_data irqd; 108442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 108542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru int fd = (int) inv_get_serial_handle(); 108642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru read(fd, &irqd, sizeof(irqd)); 108742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 108842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) { 108942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //going to sleep 109042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru sleepEvent(); 109142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) { 109242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru //waking up 109342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru wakeEvent(); 109442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 109542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 109642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru ioctl(fd, MPU_PM_EVENT_HANDLED, 0); 109742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 109842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 109942331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::sleepEvent() 110042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 110142331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 110242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 110342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mEnabled != 0) { 110442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mForceSleep = true; 110542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mOldEnabledMask = mEnabled; 110642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru setPowerStates(0); 110742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 110842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 110942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 111042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru 111142331858975144405f95243be8427084ee7d478dJean-Baptiste Queruvoid MPLSensor::wakeEvent() 111242331858975144405f95243be8427084ee7d478dJean-Baptiste Queru{ 111342331858975144405f95243be8427084ee7d478dJean-Baptiste Queru VFUNC_LOG; 111442331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_lock(&mMplMutex); 111542331858975144405f95243be8427084ee7d478dJean-Baptiste Queru if (mForceSleep) { 111642331858975144405f95243be8427084ee7d478dJean-Baptiste Queru setPowerStates((mOldEnabledMask | mEnabled)); 111742331858975144405f95243be8427084ee7d478dJean-Baptiste Queru } 111842331858975144405f95243be8427084ee7d478dJean-Baptiste Queru mForceSleep = false; 111942331858975144405f95243be8427084ee7d478dJean-Baptiste Queru pthread_mutex_unlock(&mMplMutex); 112042331858975144405f95243be8427084ee7d478dJean-Baptiste Queru} 1121986d38918eb72f00a67189a942da432df17e504eKevin Powell 1122986d38918eb72f00a67189a942da432df17e504eKevin Powell/** fill in the sensor list based on which sensors are configured. 1123986d38918eb72f00a67189a942da432df17e504eKevin Powell * return the number of configured sensors. 1124986d38918eb72f00a67189a942da432df17e504eKevin Powell * parameter list must point to a memory region of at least 7*sizeof(sensor_t) 1125986d38918eb72f00a67189a942da432df17e504eKevin Powell * parameter len gives the length of the buffer pointed to by list 1126986d38918eb72f00a67189a942da432df17e504eKevin Powell */ 1127986d38918eb72f00a67189a942da432df17e504eKevin Powell 1128986d38918eb72f00a67189a942da432df17e504eKevin Powellint MPLSensor::populateSensorList(struct sensor_t *list, int len) 1129986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1130986d38918eb72f00a67189a942da432df17e504eKevin Powell int numsensors; 1131986d38918eb72f00a67189a942da432df17e504eKevin Powell 1132986d38918eb72f00a67189a942da432df17e504eKevin Powell if(len < 7*sizeof(sensor_t)) { 1133986d38918eb72f00a67189a942da432df17e504eKevin Powell LOGE("sensor list too small, not populating."); 1134986d38918eb72f00a67189a942da432df17e504eKevin Powell return 0; 1135986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1136986d38918eb72f00a67189a942da432df17e504eKevin Powell 1137986d38918eb72f00a67189a942da432df17e504eKevin Powell /* fill in the base values */ 1138986d38918eb72f00a67189a942da432df17e504eKevin Powell memcpy(list, sSensorList, sizeof (struct sensor_t) * 7); 1139986d38918eb72f00a67189a942da432df17e504eKevin Powell 1140986d38918eb72f00a67189a942da432df17e504eKevin Powell /* first add gyro, accel and compass to the list */ 1141986d38918eb72f00a67189a942da432df17e504eKevin Powell 1142986d38918eb72f00a67189a942da432df17e504eKevin Powell /* fill in accel values */ 1143be6c0220038706c329ea7e40bf07b1fa130977c8Rosa unsigned short accelId = inv_get_accel_id(); 1144be6c0220038706c329ea7e40bf07b1fa130977c8Rosa if(accelId == 0) 1145be6c0220038706c329ea7e40bf07b1fa130977c8Rosa { 1146be6c0220038706c329ea7e40bf07b1fa130977c8Rosa LOGE("Can not get accel id"); 1147be6c0220038706c329ea7e40bf07b1fa130977c8Rosa } 1148be6c0220038706c329ea7e40bf07b1fa130977c8Rosa fillAccel(accelId, list); 1149986d38918eb72f00a67189a942da432df17e504eKevin Powell 1150986d38918eb72f00a67189a942da432df17e504eKevin Powell /* fill in compass values */ 1151be6c0220038706c329ea7e40bf07b1fa130977c8Rosa unsigned short compassId = inv_get_compass_id(); 1152be6c0220038706c329ea7e40bf07b1fa130977c8Rosa if(compassId == 0) 1153be6c0220038706c329ea7e40bf07b1fa130977c8Rosa { 1154be6c0220038706c329ea7e40bf07b1fa130977c8Rosa LOGE("Can not get compass id"); 1155be6c0220038706c329ea7e40bf07b1fa130977c8Rosa } 1156be6c0220038706c329ea7e40bf07b1fa130977c8Rosa fillCompass(compassId, list); 1157986d38918eb72f00a67189a942da432df17e504eKevin Powell 1158986d38918eb72f00a67189a942da432df17e504eKevin Powell /* fill in gyro values */ 1159986d38918eb72f00a67189a942da432df17e504eKevin Powell fillGyro(MPU_NAME, list); 1160986d38918eb72f00a67189a942da432df17e504eKevin Powell 1161986d38918eb72f00a67189a942da432df17e504eKevin Powell if(mNineAxisEnabled) 1162986d38918eb72f00a67189a942da432df17e504eKevin Powell { 1163986d38918eb72f00a67189a942da432df17e504eKevin Powell numsensors = 7; 1164986d38918eb72f00a67189a942da432df17e504eKevin Powell /* all sensors will be added to the list */ 1165986d38918eb72f00a67189a942da432df17e504eKevin Powell /* fill in orientation values */ 1166986d38918eb72f00a67189a942da432df17e504eKevin Powell fillOrientation(list); 1167986d38918eb72f00a67189a942da432df17e504eKevin Powell 1168986d38918eb72f00a67189a942da432df17e504eKevin Powell /* fill in rotation vector values */ 1169986d38918eb72f00a67189a942da432df17e504eKevin Powell fillRV(list); 1170986d38918eb72f00a67189a942da432df17e504eKevin Powell 1171986d38918eb72f00a67189a942da432df17e504eKevin Powell /* fill in gravity values */ 1172986d38918eb72f00a67189a942da432df17e504eKevin Powell fillGravity(list); 1173986d38918eb72f00a67189a942da432df17e504eKevin Powell 1174986d38918eb72f00a67189a942da432df17e504eKevin Powell /* fill in Linear accel values */ 1175986d38918eb72f00a67189a942da432df17e504eKevin Powell fillLinearAccel(list); 1176986d38918eb72f00a67189a942da432df17e504eKevin Powell } else { 1177986d38918eb72f00a67189a942da432df17e504eKevin Powell /* no 9-axis sensors, zero fill that part of the list */ 1178986d38918eb72f00a67189a942da432df17e504eKevin Powell numsensors = 3; 1179986d38918eb72f00a67189a942da432df17e504eKevin Powell memset(list+3, 0, 4*sizeof(struct sensor_t)); 1180986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1181986d38918eb72f00a67189a942da432df17e504eKevin Powell 1182986d38918eb72f00a67189a942da432df17e504eKevin Powell return numsensors; 1183986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1184986d38918eb72f00a67189a942da432df17e504eKevin Powell 1185986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillAccel(unsigned char accel, struct sensor_t *list) 1186986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1187986d38918eb72f00a67189a942da432df17e504eKevin Powell switch (accel) { 1188986d38918eb72f00a67189a942da432df17e504eKevin Powell case ACCEL_ID_LIS331: 1189986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_LIS331_RANGE; 1190986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].resolution = ACCEL_LIS331_RESOLUTION; 1191986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].power = ACCEL_LIS331_POWER; 1192986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1193986d38918eb72f00a67189a942da432df17e504eKevin Powell 1194986d38918eb72f00a67189a942da432df17e504eKevin Powell case ACCEL_ID_LIS3DH: 1195986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_LIS3DH_RANGE; 1196986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].resolution = ACCEL_LIS3DH_RESOLUTION; 1197986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].power = ACCEL_LIS3DH_POWER; 1198986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1199986d38918eb72f00a67189a942da432df17e504eKevin Powell 1200986d38918eb72f00a67189a942da432df17e504eKevin Powell case ACCEL_ID_KXSD9: 1201986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_KXSD9_RANGE; 1202986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].resolution = ACCEL_KXSD9_RESOLUTION; 1203986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].power = ACCEL_KXSD9_POWER; 1204986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1205986d38918eb72f00a67189a942da432df17e504eKevin Powell 1206986d38918eb72f00a67189a942da432df17e504eKevin Powell case ACCEL_ID_KXTF9: 1207986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_KXTF9_RANGE; 1208986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].resolution = ACCEL_KXTF9_RESOLUTION; 1209986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].power = ACCEL_KXTF9_POWER; 1210986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1211986d38918eb72f00a67189a942da432df17e504eKevin Powell 1212986d38918eb72f00a67189a942da432df17e504eKevin Powell case ACCEL_ID_BMA150: 1213986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_BMA150_RANGE; 1214986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].resolution = ACCEL_BMA150_RESOLUTION; 1215986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].power = ACCEL_BMA150_POWER; 1216986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1217986d38918eb72f00a67189a942da432df17e504eKevin Powell 1218986d38918eb72f00a67189a942da432df17e504eKevin Powell case ACCEL_ID_BMA222: 1219986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_BMA222_RANGE; 1220986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].resolution = ACCEL_BMA222_RESOLUTION; 1221986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].power = ACCEL_BMA222_POWER; 1222986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1223986d38918eb72f00a67189a942da432df17e504eKevin Powell 1224986d38918eb72f00a67189a942da432df17e504eKevin Powell case ACCEL_ID_BMA250: 1225986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; 1226986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; 1227986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].power = ACCEL_BMA250_POWER; 1228986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1229986d38918eb72f00a67189a942da432df17e504eKevin Powell 1230986d38918eb72f00a67189a942da432df17e504eKevin Powell case ACCEL_ID_ADXL34X: 1231986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_ADXL34X_RANGE; 1232986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].resolution = ACCEL_ADXL34X_RESOLUTION; 1233986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].power = ACCEL_ADXL34X_POWER; 1234986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1235986d38918eb72f00a67189a942da432df17e504eKevin Powell 1236986d38918eb72f00a67189a942da432df17e504eKevin Powell case ACCEL_ID_MMA8450: 1237986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1238986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1239986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; 1240986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1241986d38918eb72f00a67189a942da432df17e504eKevin Powell 1242986d38918eb72f00a67189a942da432df17e504eKevin Powell case ACCEL_ID_MMA845X: 1243986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_MMA845X_RANGE; 1244986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].resolution = ACCEL_MMA845X_RESOLUTION; 1245986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].power = ACCEL_MMA845X_POWER; 1246986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1247986d38918eb72f00a67189a942da432df17e504eKevin Powell 1248986d38918eb72f00a67189a942da432df17e504eKevin Powell case ACCEL_ID_MPU6050: 1249986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE; 1250986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION; 1251986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Accelerometer].power = ACCEL_MPU6050_POWER; 1252986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1253986d38918eb72f00a67189a942da432df17e504eKevin Powell default: 1254986d38918eb72f00a67189a942da432df17e504eKevin Powell LOGE("unknown accel id -- accel params will be wrong."); 1255986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1256986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1257986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1258986d38918eb72f00a67189a942da432df17e504eKevin Powell 1259986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillCompass(unsigned char compass, struct sensor_t *list) 1260986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1261986d38918eb72f00a67189a942da432df17e504eKevin Powell switch (compass) { 1262986d38918eb72f00a67189a942da432df17e504eKevin Powell case COMPASS_ID_AK8975: 1263986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].maxRange = COMPASS_AKM8975_RANGE; 1264986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].resolution = COMPASS_AKM8975_RESOLUTION; 1265986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].power = COMPASS_AKM8975_POWER; 1266986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1267986d38918eb72f00a67189a942da432df17e504eKevin Powell case COMPASS_ID_AMI30X: 1268986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].maxRange = COMPASS_AMI30X_RANGE; 1269986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].resolution = COMPASS_AMI30X_RESOLUTION; 1270986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].power = COMPASS_AMI30X_POWER; 1271986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1272986d38918eb72f00a67189a942da432df17e504eKevin Powell case COMPASS_ID_AMI306: 1273986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].maxRange = COMPASS_AMI306_RANGE; 1274986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; 1275986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].power = COMPASS_AMI306_POWER; 1276986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1277986d38918eb72f00a67189a942da432df17e504eKevin Powell case COMPASS_ID_YAS529: 1278986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].maxRange = COMPASS_YAS529_RANGE; 1279986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; 1280986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].power = COMPASS_AMI306_POWER; 1281986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1282986d38918eb72f00a67189a942da432df17e504eKevin Powell case COMPASS_ID_YAS530: 1283986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].maxRange = COMPASS_YAS530_RANGE; 1284986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION; 1285986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].power = COMPASS_YAS530_POWER; 1286986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1287986d38918eb72f00a67189a942da432df17e504eKevin Powell case COMPASS_ID_HMC5883: 1288986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].maxRange = COMPASS_HMC5883_RANGE; 1289986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].resolution = COMPASS_HMC5883_RESOLUTION; 1290986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].power = COMPASS_HMC5883_POWER; 1291986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1292986d38918eb72f00a67189a942da432df17e504eKevin Powell case COMPASS_ID_MMC314X: 1293986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].maxRange = COMPASS_MMC314X_RANGE; 1294986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].resolution = COMPASS_MMC314X_RESOLUTION; 1295986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].power = COMPASS_MMC314X_POWER; 1296986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1297986d38918eb72f00a67189a942da432df17e504eKevin Powell case COMPASS_ID_HSCDTD002B: 1298986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].maxRange = COMPASS_HSCDTD002B_RANGE; 1299986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].resolution = COMPASS_HSCDTD002B_RESOLUTION; 1300986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].power = COMPASS_HSCDTD002B_POWER; 1301986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1302986d38918eb72f00a67189a942da432df17e504eKevin Powell case COMPASS_ID_HSCDTD004A: 1303986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].maxRange = COMPASS_HSCDTD004A_RANGE; 1304986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].resolution = COMPASS_HSCDTD004A_RESOLUTION; 1305986d38918eb72f00a67189a942da432df17e504eKevin Powell list[MagneticField].power = COMPASS_HSCDTD004A_POWER; 1306986d38918eb72f00a67189a942da432df17e504eKevin Powell break; 1307986d38918eb72f00a67189a942da432df17e504eKevin Powell default: 1308986d38918eb72f00a67189a942da432df17e504eKevin Powell LOGE("unknown compass id -- compass parameters will be wrong"); 1309986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1310986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1311986d38918eb72f00a67189a942da432df17e504eKevin Powell 1312986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) 1313986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1314986d38918eb72f00a67189a942da432df17e504eKevin Powell if ((gyro != NULL) && (strcmp(gyro, "mpu3050") == 0)) { 1315986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].maxRange = GYRO_MPU3050_RANGE; 1316986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; 1317986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].power = GYRO_MPU3050_POWER; 1318986d38918eb72f00a67189a942da432df17e504eKevin Powell } else { 1319986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].maxRange = GYRO_MPU6050_RANGE; 1320986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].resolution = GYRO_MPU6050_RESOLUTION; 1321986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gyro].power = GYRO_MPU6050_POWER; 1322986d38918eb72f00a67189a942da432df17e504eKevin Powell } 1323986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1324986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1325986d38918eb72f00a67189a942da432df17e504eKevin Powell 1326986d38918eb72f00a67189a942da432df17e504eKevin Powell 1327986d38918eb72f00a67189a942da432df17e504eKevin Powell/* fillRV depends on values of accel and compass in the list */ 1328986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillRV(struct sensor_t *list) 1329986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1330986d38918eb72f00a67189a942da432df17e504eKevin Powell /* compute power on the fly */ 1331986d38918eb72f00a67189a942da432df17e504eKevin Powell list[RotationVector].power = list[Gyro].power + list[Accelerometer].power 1332986d38918eb72f00a67189a942da432df17e504eKevin Powell + list[MagneticField].power; 1333986d38918eb72f00a67189a942da432df17e504eKevin Powell list[RotationVector].resolution = .00001; 1334986d38918eb72f00a67189a942da432df17e504eKevin Powell list[RotationVector].maxRange = 1.0; 1335986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1336986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1337986d38918eb72f00a67189a942da432df17e504eKevin Powell 1338986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillOrientation(struct sensor_t *list) 1339986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1340986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Orientation].power = list[Gyro].power + list[Accelerometer].power 1341986d38918eb72f00a67189a942da432df17e504eKevin Powell + list[MagneticField].power; 1342986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Orientation].resolution = .00001; 1343986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Orientation].maxRange = 360.0; 1344986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1345986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1346986d38918eb72f00a67189a942da432df17e504eKevin Powell 1347986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillGravity( struct sensor_t *list) 1348986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1349986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gravity].power = list[Gyro].power + list[Accelerometer].power 1350986d38918eb72f00a67189a942da432df17e504eKevin Powell + list[MagneticField].power; 1351986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gravity].resolution = .00001; 1352986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gravity].maxRange = 9.81; 1353986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1354986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1355986d38918eb72f00a67189a942da432df17e504eKevin Powell 1356986d38918eb72f00a67189a942da432df17e504eKevin Powellvoid MPLSensor::fillLinearAccel(struct sensor_t *list) 1357986d38918eb72f00a67189a942da432df17e504eKevin Powell{ 1358986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gravity].power = list[Gyro].power + list[Accelerometer].power 1359986d38918eb72f00a67189a942da432df17e504eKevin Powell + list[MagneticField].power; 1360986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gravity].resolution = list[Accelerometer].resolution; 1361986d38918eb72f00a67189a942da432df17e504eKevin Powell list[Gravity].maxRange = list[Accelerometer].maxRange; 1362986d38918eb72f00a67189a942da432df17e504eKevin Powell return; 1363986d38918eb72f00a67189a942da432df17e504eKevin Powell} 1364986d38918eb72f00a67189a942da432df17e504eKevin Powell 1365