Home | History | Annotate | Download | only in libsensors
      1 /*
      2  * Copyright (C) 2011 Invensense, Inc.
      3  *
      4  * Licensed under the Apache License, Version 2.0 (the "License");
      5  * you may not use this file except in compliance with the License.
      6  * You may obtain a copy of the License at
      7  *
      8  *      http://www.apache.org/licenses/LICENSE-2.0
      9  *
     10  * Unless required by applicable law or agreed to in writing, software
     11  * distributed under the License is distributed on an "AS IS" BASIS,
     12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     13  * See the License for the specific language governing permissions and
     14  * limitations under the License.
     15  */
     16 
     17 //#define LOG_NDEBUG 0
     18 //see also the EXTRA_VERBOSE define, below
     19 
     20 #include <fcntl.h>
     21 #include <errno.h>
     22 #include <math.h>
     23 #include <float.h>
     24 #include <poll.h>
     25 #include <unistd.h>
     26 #include <dirent.h>
     27 #include <stdlib.h>
     28 #include <sys/select.h>
     29 #include <dlfcn.h>
     30 #include <pthread.h>
     31 
     32 #include <cutils/log.h>
     33 #include <utils/KeyedVector.h>
     34 
     35 #include "MPLSensor.h"
     36 
     37 #include "math.h"
     38 #include "ml.h"
     39 #include "mlFIFO.h"
     40 #include "mlsl.h"
     41 #include "mlos.h"
     42 #include "ml_mputest.h"
     43 #include "ml_stored_data.h"
     44 #include "mldl_cfg.h"
     45 #include "mldl.h"
     46 
     47 #include "mpu.h"
     48 #include "accel.h"
     49 #include "compass.h"
     50 #include "kernel/timerirq.h"
     51 #include "kernel/mpuirq.h"
     52 #include "kernel/slaveirq.h"
     53 
     54 extern "C" {
     55 #include "mlsupervisor.h"
     56 }
     57 
     58 #include "mlcontrol.h"
     59 #include "sensor_params.h"
     60 
     61 #define EXTRA_VERBOSE (0)
     62 //#define FUNC_LOG ALOGV("%s", __PRETTY_FUNCTION__)
     63 #define FUNC_LOG
     64 #define VFUNC_LOG ALOGV_IF(EXTRA_VERBOSE, "%s", __PRETTY_FUNCTION__)
     65 /* this mask must turn on only the sensors that are present and managed by the MPL */
     66 #define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_COMPASS | INV_THREE_AXIS_GYRO)
     67 
     68 #define CALL_MEMBER_FN(pobject,ptrToMember)  ((pobject)->*(ptrToMember))
     69 
     70 /******************************************/
     71 
     72 /* Base values for the sensor list, these need to be in the order defined in MPLSensor.h */
     73 static struct sensor_t sSensorList[] =
     74     { { "MPL Gyroscope", "Invensense", 1,
     75          SENSORS_GYROSCOPE_HANDLE,
     76          SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
     77       { "MPL Accelerometer", "Invensense", 1,
     78          SENSORS_ACCELERATION_HANDLE,
     79          SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
     80       { "MPL Magnetic Field", "Invensense", 1,
     81          SENSORS_MAGNETIC_FIELD_HANDLE,
     82          SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
     83       { "MPL Orientation", "Invensense", 1,
     84          SENSORS_ORIENTATION_HANDLE,
     85          SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, { } },
     86       { "MPL Rotation Vector", "Invensense", 1,
     87          SENSORS_ROTATION_VECTOR_HANDLE,
     88          SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
     89       { "MPL Linear Acceleration", "Invensense", 1,
     90          SENSORS_LINEAR_ACCEL_HANDLE,
     91          SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
     92       { "MPL Gravity", "Invensense", 1,
     93          SENSORS_GRAVITY_HANDLE,
     94          SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } },
     95 };
     96 
     97 static unsigned long long irq_timestamp = 0;
     98 /* ***************************************************************************
     99  * MPL interface misc.
    100  */
    101 //static pointer to the object that will handle callbacks
    102 static MPLSensor* gMPLSensor = NULL;
    103 
    104 /* we need to pass some callbacks to the MPL.  The mpl is a C library, so
    105  * wrappers for the C++ callback implementations are required.
    106  */
    107 extern "C" {
    108 //callback wrappers go here
    109 void mot_cb_wrapper(uint16_t val)
    110 {
    111     if (gMPLSensor) {
    112         gMPLSensor->cbOnMotion(val);
    113     }
    114 }
    115 
    116 void procData_cb_wrapper()
    117 {
    118     if(gMPLSensor) {
    119         gMPLSensor->cbProcData();
    120     }
    121 }
    122 
    123 } //end of extern C
    124 
    125 void setCallbackObject(MPLSensor* gbpt)
    126 {
    127     gMPLSensor = gbpt;
    128 }
    129 
    130 
    131 /*****************************************************************************
    132  * sensor class implementation
    133  */
    134 
    135 #define GY_ENABLED ((1<<ID_GY) & enabled_sensors)
    136 #define A_ENABLED  ((1<<ID_A)  & enabled_sensors)
    137 #define O_ENABLED  ((1<<ID_O)  & enabled_sensors)
    138 #define M_ENABLED  ((1<<ID_M)  & enabled_sensors)
    139 #define LA_ENABLED ((1<<ID_LA) & enabled_sensors)
    140 #define GR_ENABLED ((1<<ID_GR) & enabled_sensors)
    141 #define RV_ENABLED ((1<<ID_RV) & enabled_sensors)
    142 
    143 MPLSensor::MPLSensor() :
    144     SensorBase(NULL, NULL),
    145             mNewData(0),
    146             mDmpStarted(false),
    147             mMasterSensorMask(INV_ALL_SENSORS),
    148             mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1),
    149             mCurFifoRate(-1), mHaveGoodMpuCal(false), mHaveGoodCompassCal(false),
    150             mUseTimerIrqAccel(false), mUsetimerIrqCompass(true),
    151             mUseTimerirq(false), mSampleCount(0),
    152             mEnabled(0), mPendingMask(0)
    153 {
    154     FUNC_LOG;
    155     inv_error_t rv;
    156     int mpu_int_fd, i;
    157     char *port = NULL;
    158 
    159     ALOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors);
    160 
    161     pthread_mutex_init(&mMplMutex, NULL);
    162 
    163     mForceSleep = false;
    164 
    165     /* used for identifying whether 9axis is enabled or not             */
    166     /* this variable will be changed in initMPL() when libmpl is loaded */
    167     /* sensor list will be changed based on this variable               */
    168     mNineAxisEnabled = false;
    169 
    170     for (i = 0; i < ARRAY_SIZE(mPollFds); i++) {
    171         mPollFds[i].fd = -1;
    172         mPollFds[i].events = 0;
    173     }
    174 
    175     pthread_mutex_lock(&mMplMutex);
    176 
    177     mpu_int_fd = open("/dev/mpuirq", O_RDWR);
    178     if (mpu_int_fd == -1) {
    179         ALOGE("could not open the mpu irq device node");
    180     } else {
    181         fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK);
    182         //ioctl(mpu_int_fd, MPUIRQ_SET_TIMEOUT, 0);
    183         mIrqFds.add(MPUIRQ_FD, mpu_int_fd);
    184         mPollFds[MPUIRQ_FD].fd = mpu_int_fd;
    185         mPollFds[MPUIRQ_FD].events = POLLIN;
    186     }
    187 
    188     accel_fd = open("/dev/accelirq", O_RDWR);
    189     if (accel_fd == -1) {
    190         ALOGE("could not open the accel irq device node");
    191     } else {
    192         fcntl(accel_fd, F_SETFL, O_NONBLOCK);
    193         //ioctl(accel_fd, SLAVEIRQ_SET_TIMEOUT, 0);
    194         mIrqFds.add(ACCELIRQ_FD, accel_fd);
    195         mPollFds[ACCELIRQ_FD].fd = accel_fd;
    196         mPollFds[ACCELIRQ_FD].events = POLLIN;
    197     }
    198 
    199     timer_fd = open("/dev/timerirq", O_RDWR);
    200     if (timer_fd == -1) {
    201         ALOGE("could not open the timer irq device node");
    202     } else {
    203         fcntl(timer_fd, F_SETFL, O_NONBLOCK);
    204         //ioctl(timer_fd, TIMERIRQ_SET_TIMEOUT, 0);
    205         mIrqFds.add(TIMERIRQ_FD, timer_fd);
    206         mPollFds[TIMERIRQ_FD].fd = timer_fd;
    207         mPollFds[TIMERIRQ_FD].events = POLLIN;
    208     }
    209 
    210     data_fd = mpu_int_fd;
    211 
    212     if ((accel_fd == -1) && (timer_fd != -1)) {
    213         //no accel irq and timer available
    214         mUseTimerIrqAccel = true;
    215         //ALOGD("MPLSensor falling back to timerirq for accel data");
    216     }
    217 
    218     memset(mPendingEvents, 0, sizeof(mPendingEvents));
    219 
    220     mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
    221     mPendingEvents[RotationVector].sensor = ID_RV;
    222     mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
    223 
    224     mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
    225     mPendingEvents[LinearAccel].sensor = ID_LA;
    226     mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
    227 
    228     mPendingEvents[Gravity].version = sizeof(sensors_event_t);
    229     mPendingEvents[Gravity].sensor = ID_GR;
    230     mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
    231 
    232     mPendingEvents[Gyro].version = sizeof(sensors_event_t);
    233     mPendingEvents[Gyro].sensor = ID_GY;
    234     mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
    235 
    236     mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
    237     mPendingEvents[Accelerometer].sensor = ID_A;
    238     mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
    239 
    240     mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
    241     mPendingEvents[MagneticField].sensor = ID_M;
    242     mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
    243     mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
    244 
    245     mPendingEvents[Orientation].version = sizeof(sensors_event_t);
    246     mPendingEvents[Orientation].sensor = ID_O;
    247     mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
    248     mPendingEvents[Orientation].orientation.status
    249             = SENSOR_STATUS_ACCURACY_HIGH;
    250 
    251     mHandlers[RotationVector] = &MPLSensor::rvHandler;
    252     mHandlers[LinearAccel] = &MPLSensor::laHandler;
    253     mHandlers[Gravity] = &MPLSensor::gravHandler;
    254     mHandlers[Gyro] = &MPLSensor::gyroHandler;
    255     mHandlers[Accelerometer] = &MPLSensor::accelHandler;
    256     mHandlers[MagneticField] = &MPLSensor::compassHandler;
    257     mHandlers[Orientation] = &MPLSensor::orienHandler;
    258 
    259     for (int i = 0; i < numSensors; i++)
    260         mDelays[i] = 30000000LLU; // 30 ms by default
    261 
    262     if (inv_serial_start(port) != INV_SUCCESS) {
    263         ALOGE("Fatal Error : could not open MPL serial interface");
    264     }
    265 
    266     //initialize library parameters
    267     initMPL();
    268 
    269     //setup the FIFO contents
    270     setupFIFO();
    271 
    272     //we start the motion processing only when a sensor is enabled...
    273     //rv = inv_dmp_start();
    274     //ALOGE_IF(rv != INV_SUCCESS, "Fatal error: could not start the DMP correctly. (code = %d)\n", rv);
    275     //dmp_started = true;
    276 
    277     pthread_mutex_unlock(&mMplMutex);
    278 
    279 }
    280 
    281 MPLSensor::~MPLSensor()
    282 {
    283     FUNC_LOG;
    284     pthread_mutex_lock(&mMplMutex);
    285     if (inv_dmp_stop() != INV_SUCCESS) {
    286         ALOGW("Error: could not stop the DMP correctly.\n");
    287     }
    288 
    289     if (inv_dmp_close() != INV_SUCCESS) {
    290         ALOGW("Error: could not close the DMP");
    291     }
    292 
    293     if (inv_serial_stop() != INV_SUCCESS) {
    294         ALOGW("Error : could not close the serial port");
    295     }
    296     pthread_mutex_unlock(&mMplMutex);
    297     pthread_mutex_destroy(&mMplMutex);
    298 }
    299 
    300 /* clear any data from our various filehandles */
    301 void MPLSensor::clearIrqData(bool* irq_set)
    302 {
    303     unsigned int i;
    304     int nread;
    305     struct mpuirq_data irqdata;
    306 
    307     poll(mPollFds, ARRAY_SIZE(mPollFds), 0); //check which ones need to be cleared
    308 
    309     for (i = 0; i < ARRAY_SIZE(mPollFds); i++) {
    310         int cur_fd = mPollFds[i].fd;
    311         int j = 0;
    312         if (mPollFds[i].revents & POLLIN) {
    313             nread = read(cur_fd, &irqdata, sizeof(irqdata));
    314             if (nread > 0) {
    315                 irq_set[i] = true;
    316                 irq_timestamp = irqdata.irqtime;
    317                 //ALOGV_IF(EXTRA_VERBOSE, "irq: %d %d (%d)", i, irqdata.interruptcount, j++);
    318             }
    319         }
    320         mPollFds[i].revents = 0;
    321     }
    322 }
    323 
    324 /* set the power states of the various sensors based on the bits set in the
    325  * enabled_sensors parameter.
    326  * this function modifies globalish state variables.  It must be called with the mMplMutex held. */
    327 void MPLSensor::setPowerStates(int enabled_sensors)
    328 {
    329     FUNC_LOG;
    330     bool irq_set[5] = { false, false, false, false, false };
    331 
    332     //ALOGV(" setPowerStates: %d dmp_started: %d", enabled_sensors, mDmpStarted);
    333 
    334     do {
    335 
    336         if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
    337             mLocalSensorMask = ALL_MPL_SENSORS_NP;
    338             break;
    339         }
    340 
    341         if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
    342             mLocalSensorMask = 0;
    343             break;
    344         }
    345 
    346         if (GY_ENABLED) {
    347             mLocalSensorMask |= INV_THREE_AXIS_GYRO;
    348         } else {
    349             mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
    350         }
    351 
    352         if (A_ENABLED) {
    353             mLocalSensorMask |= (INV_THREE_AXIS_ACCEL);
    354         } else {
    355             mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL);
    356         }
    357 
    358         if (M_ENABLED) {
    359             mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
    360         } else {
    361             mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS);
    362         }
    363 
    364     } while (0);
    365 
    366     //record the new sensor state
    367     inv_error_t rv;
    368 
    369     long sen_mask = mLocalSensorMask & mMasterSensorMask;
    370 
    371     bool changing_sensors = ((inv_get_dl_config()->requested_sensors
    372             != sen_mask) && (sen_mask != 0));
    373     bool restart = (!mDmpStarted) && (sen_mask != 0);
    374 
    375     if (changing_sensors || restart) {
    376 
    377         ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart);
    378 
    379         if (mDmpStarted) {
    380             inv_dmp_stop();
    381             clearIrqData(irq_set);
    382             mDmpStarted = false;
    383         }
    384 
    385         if (sen_mask != inv_get_dl_config()->requested_sensors) {
    386             //ALOGV("setPowerStates: %lx", sen_mask);
    387             rv = inv_set_mpu_sensors(sen_mask);
    388             ALOGE_IF(rv != INV_SUCCESS,
    389                     "error: unable to set MPL sensor power states (sens=%ld retcode = %d)",
    390                     sen_mask, rv);
    391         }
    392 
    393         if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS))
    394                 || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL)))
    395                 && ((sen_mask & INV_DMP_PROCESSOR) == 0)) {
    396             ALOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ");
    397             mUseTimerirq = true;
    398         } else {
    399             if (mUseTimerirq) {
    400                 ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
    401                 clearIrqData(irq_set);
    402             }
    403             ALOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ");
    404             mUseTimerirq = false;
    405         }
    406 
    407         if (!mDmpStarted) {
    408             if (mHaveGoodMpuCal || mHaveGoodCompassCal) {
    409                 rv = inv_store_calibration();
    410                 ALOGE_IF(rv != INV_SUCCESS,
    411                         "error: unable to store MPL calibration file");
    412                 mHaveGoodMpuCal = false;
    413                 mHaveGoodCompassCal = false;
    414             }
    415             //ALOGV("Starting DMP");
    416             rv = inv_dmp_start();
    417             ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp");
    418             mDmpStarted = true;
    419         }
    420     }
    421 
    422     //check if we should stop the DMP
    423     if (mDmpStarted && (sen_mask == 0)) {
    424         //ALOGV("Stopping DMP");
    425         rv = inv_dmp_stop();
    426         ALOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)",
    427                 rv);
    428         if (mUseTimerirq) {
    429             ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
    430         }
    431         clearIrqData(irq_set);
    432 
    433         mDmpStarted = false;
    434         mPollTime = -1;
    435         mCurFifoRate = -1;
    436     }
    437 
    438 }
    439 
    440 /**
    441  * container function for all the calls we make once to set up the MPL.
    442  */
    443 void MPLSensor::initMPL()
    444 {
    445     FUNC_LOG;
    446     inv_error_t result;
    447     unsigned short bias_update_mask = 0xFFFF;
    448     struct mldl_cfg *mldl_cfg;
    449 
    450     if (inv_dmp_open() != INV_SUCCESS) {
    451         ALOGE("Fatal Error : could not open DMP correctly.\n");
    452     }
    453 
    454     result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work
    455     ALOGE_IF(result != INV_SUCCESS,
    456             "Fatal Error : could not set enabled sensors.");
    457 
    458     if (inv_load_calibration() != INV_SUCCESS) {
    459         ALOGE("could not open MPL calibration file");
    460     }
    461 
    462     //check for the 9axis fusion library: if available load it and start 9x
    463     void* h_dmp_lib=dlopen("libinvensense_mpl.so", RTLD_NOW);
    464     if(h_dmp_lib) {
    465         const char* error;
    466         error = dlerror();
    467         inv_error_t (*fp_inv_enable_9x_fusion)() =
    468               (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion");
    469         if((error = dlerror()) != NULL) {
    470             ALOGE("%s %s", error, "inv_enable_9x_fusion");
    471         } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) {
    472             ALOGE( "Warning : 9 axis sensor fusion not available "
    473                   "- No compass detected.\n");
    474         } else {
    475             /*  9axis is loaded and enabled                            */
    476             /*  this variable is used for coming up with sensor list   */
    477             mNineAxisEnabled = true;
    478         }
    479     } else {
    480         const char* error = dlerror();
    481         ALOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error);
    482     }
    483 
    484     mldl_cfg = inv_get_dl_config();
    485 
    486     if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) {
    487         ALOGE("Error : Bias update function could not be set.\n");
    488     }
    489 
    490     if (inv_set_motion_interrupt(1) != INV_SUCCESS) {
    491         ALOGE("Error : could not set motion interrupt");
    492     }
    493 
    494     if (inv_set_fifo_interrupt(1) != INV_SUCCESS) {
    495         ALOGE("Error : could not set fifo interrupt");
    496     }
    497 
    498     result = inv_set_fifo_rate(6);
    499     if (result != INV_SUCCESS) {
    500         ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result);
    501     }
    502 
    503     setupCallbacks();
    504 
    505 }
    506 
    507 /** setup the fifo contents.
    508  */
    509 void MPLSensor::setupFIFO()
    510 {
    511     FUNC_LOG;
    512     inv_error_t result;
    513 
    514     result = inv_send_accel(INV_ALL, INV_32_BIT);
    515     if (result != INV_SUCCESS) {
    516         ALOGE("Fatal error: inv_send_accel returned %d\n", result);
    517     }
    518 
    519     result = inv_send_quaternion(INV_32_BIT);
    520     if (result != INV_SUCCESS) {
    521         ALOGE("Fatal error: inv_send_quaternion returned %d\n", result);
    522     }
    523 
    524     result = inv_send_linear_accel(INV_ALL, INV_32_BIT);
    525     if (result != INV_SUCCESS) {
    526         ALOGE("Fatal error: inv_send_linear_accel returned %d\n", result);
    527     }
    528 
    529     result = inv_send_linear_accel_in_world(INV_ALL, INV_32_BIT);
    530     if (result != INV_SUCCESS) {
    531         ALOGE("Fatal error: inv_send_linear_accel_in_world returned %d\n",
    532              result);
    533     }
    534 
    535     result = inv_send_gravity(INV_ALL, INV_32_BIT);
    536     if (result != INV_SUCCESS) {
    537         ALOGE("Fatal error: inv_send_gravity returned %d\n", result);
    538     }
    539 
    540     result = inv_send_gyro(INV_ALL, INV_32_BIT);
    541     if (result != INV_SUCCESS) {
    542         ALOGE("Fatal error: inv_send_gyro returned %d\n", result);
    543     }
    544 
    545 }
    546 
    547 /**
    548  *  set up the callbacks that we use in all cases (outside of gestures, etc)
    549  */
    550 void MPLSensor::setupCallbacks()
    551 {
    552     FUNC_LOG;
    553     if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) {
    554         ALOGE("Error : Motion callback could not be set.\n");
    555 
    556     }
    557 
    558     if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) {
    559         ALOGE("Error : Processed data callback could not be set.");
    560 
    561     }
    562 }
    563 
    564 /**
    565  * handle the motion/no motion output from the MPL.
    566  */
    567 void MPLSensor::cbOnMotion(uint16_t val)
    568 {
    569     FUNC_LOG;
    570     //after the first no motion, the gyro should be calibrated well
    571     if (val == 2) {
    572         if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) {
    573             //if gyros are on and we got a no motion, set a flag
    574             // indicating that the cal file can be written.
    575             mHaveGoodMpuCal = true;
    576         }
    577     }
    578 
    579     return;
    580 }
    581 
    582 
    583 void MPLSensor::cbProcData()
    584 {
    585     mNewData = 1;
    586     mSampleCount++;
    587     //ALOGV_IF(EXTRA_VERBOSE, "new data (%d)", sampleCount);
    588 }
    589 
    590 //these handlers transform mpl data into one of the Android sensor types
    591 //  scaling and coordinate transforms should be done in the handlers
    592 
    593 void MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask,
    594                              int index)
    595 {
    596     VFUNC_LOG;
    597     inv_error_t res;
    598     res = inv_get_float_array(INV_GYROS, s->gyro.v);
    599     s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0;
    600     s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0;
    601     s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0;
    602     if (res == INV_SUCCESS)
    603         *pending_mask |= (1 << index);
    604 }
    605 
    606 void MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask,
    607                               int index)
    608 {
    609     //VFUNC_LOG;
    610     inv_error_t res;
    611     res = inv_get_float_array(INV_ACCELS, s->acceleration.v);
    612     //res = inv_get_accel_float(s->acceleration.v);
    613     s->acceleration.v[0] = s->acceleration.v[0] * 9.81;
    614     s->acceleration.v[1] = s->acceleration.v[1] * 9.81;
    615     s->acceleration.v[2] = s->acceleration.v[2] * 9.81;
    616     //ALOGV_IF(EXTRA_VERBOSE, "accel data: %f %f %f", s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2]);
    617     if (res == INV_SUCCESS)
    618         *pending_mask |= (1 << index);
    619 }
    620 
    621 int MPLSensor::estimateCompassAccuracy()
    622 {
    623     inv_error_t res;
    624     int rv;
    625 
    626     res = inv_get_compass_accuracy(&rv);
    627     if(rv >= SENSOR_STATUS_ACCURACY_MEDIUM) {
    628          mHaveGoodCompassCal = true;
    629     }
    630     ALOGE_IF(res != INV_SUCCESS, "error returned from inv_get_compass_accuracy");
    631 
    632     return rv;
    633 }
    634 
    635 void MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask,
    636                                 int index)
    637 {
    638     VFUNC_LOG;
    639     inv_error_t res, res2;
    640     float bias_error[3];
    641     float total_be;
    642     static int bias_error_settled = 0;
    643 
    644     res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v);
    645 
    646     if (res != INV_SUCCESS) {
    647         ALOGW(
    648              "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d",
    649              res);
    650     }
    651 
    652     s->magnetic.status = estimateCompassAccuracy();
    653 
    654     if (res == INV_SUCCESS)
    655         *pending_mask |= (1 << index);
    656 }
    657 
    658 void MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask,
    659                            int index)
    660 {
    661     VFUNC_LOG;
    662     float quat[4];
    663     float norm = 0;
    664     float ang = 0;
    665     inv_error_t r;
    666 
    667     r = inv_get_float_array(INV_QUATERNION, quat);
    668 
    669     if (r != INV_SUCCESS) {
    670         *pending_mask &= ~(1 << index);
    671         return;
    672     } else {
    673         *pending_mask |= (1 << index);
    674     }
    675 
    676     norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3]
    677             + FLT_EPSILON;
    678 
    679     if (norm > 1.0f) {
    680         //renormalize
    681         norm = sqrtf(norm);
    682         float inv_norm = 1.0f / norm;
    683         quat[1] = quat[1] * inv_norm;
    684         quat[2] = quat[2] * inv_norm;
    685         quat[3] = quat[3] * inv_norm;
    686     }
    687 
    688     if (quat[0] < 0.0) {
    689         quat[1] = -quat[1];
    690         quat[2] = -quat[2];
    691         quat[3] = -quat[3];
    692     }
    693 
    694     s->gyro.v[0] = quat[1];
    695     s->gyro.v[1] = quat[2];
    696     s->gyro.v[2] = quat[3];
    697 
    698 }
    699 
    700 void MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask,
    701                            int index)
    702 {
    703     VFUNC_LOG;
    704     inv_error_t res;
    705     res = inv_get_float_array(INV_LINEAR_ACCELERATION, s->gyro.v);
    706     s->gyro.v[0] *= 9.81;
    707     s->gyro.v[1] *= 9.81;
    708     s->gyro.v[2] *= 9.81;
    709     if (res == INV_SUCCESS)
    710         *pending_mask |= (1 << index);
    711 }
    712 
    713 void MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask,
    714                              int index)
    715 {
    716     VFUNC_LOG;
    717     inv_error_t res;
    718     res = inv_get_float_array(INV_GRAVITY, s->gyro.v);
    719     s->gyro.v[0] *= 9.81;
    720     s->gyro.v[1] *= 9.81;
    721     s->gyro.v[2] *= 9.81;
    722     if (res == INV_SUCCESS)
    723         *pending_mask |= (1 << index);
    724 }
    725 
    726 void MPLSensor::calcOrientationSensor(float *R, float *values)
    727 {
    728     float tmp;
    729 
    730     //Azimuth
    731     if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) {
    732         values[0] = (float) atan2f(-R[3], R[0]);
    733     } else {
    734         values[0] = (float) atan2f(R[1], R[4]);
    735     }
    736     values[0] *= 57.295779513082320876798154814105f;
    737     if (values[0] < 0) {
    738         values[0] += 360.0f;
    739     }
    740     //Pitch
    741     tmp = R[7];
    742     if (tmp > 1.0f)
    743         tmp = 1.0f;
    744     if (tmp < -1.0f)
    745         tmp = -1.0f;
    746     values[1] = -asinf(tmp) * 57.295779513082320876798154814105f;
    747     if (R[8] < 0) {
    748         values[1] = 180.0f - values[1];
    749     }
    750     if (values[1] > 180.0f) {
    751         values[1] -= 360.0f;
    752     }
    753     //Roll
    754     if ((R[7] > 0.7071067f)) {
    755         values[2] = (float) atan2f(R[6], R[7]);
    756     } else {
    757         values[2] = (float) atan2f(R[6], R[8]);
    758     }
    759 
    760     values[2] *= 57.295779513082320876798154814105f;
    761     if (values[2] > 90.0f) {
    762         values[2] = 180.0f - values[2];
    763     }
    764     if (values[2] < -90.0f) {
    765         values[2] = -180.0f - values[2];
    766     }
    767 }
    768 
    769 void MPLSensor::orienHandler(sensors_event_t* s, uint32_t* pending_mask,
    770                               int index) //note that this is the handler for the android 'orientation' sensor, not the mpl orientation output
    771 {
    772     VFUNC_LOG;
    773     inv_error_t res;
    774     float euler[3];
    775     float heading[1];
    776     float rot_mat[9];
    777 
    778     res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat);
    779 
    780     //ComputeAndOrientation(heading[0], euler, s->orientation.v);
    781     calcOrientationSensor(rot_mat, s->orientation.v);
    782 
    783     s->orientation.status = estimateCompassAccuracy();
    784 
    785     if (res == INV_SUCCESS)
    786         *pending_mask |= (1 << index);
    787     else
    788         ALOGW("orienHandler: data not valid (%d)", (int) res);
    789 
    790 }
    791 
    792 int MPLSensor::enable(int32_t handle, int en)
    793 {
    794     FUNC_LOG;
    795     //ALOGV("handle : %d en: %d", handle, en);
    796 
    797     int what = -1;
    798 
    799     switch (handle) {
    800     case ID_A:
    801         what = Accelerometer;
    802         break;
    803     case ID_M:
    804         what = MagneticField;
    805         break;
    806     case ID_O:
    807         what = Orientation;
    808         break;
    809     case ID_GY:
    810         what = Gyro;
    811         break;
    812     case ID_GR:
    813         what = Gravity;
    814         break;
    815     case ID_RV:
    816         what = RotationVector;
    817         break;
    818     case ID_LA:
    819         what = LinearAccel;
    820         break;
    821     default: //this takes care of all the gestures
    822         what = handle;
    823         break;
    824     }
    825 
    826     if (uint32_t(what) >= numSensors)
    827         return -EINVAL;
    828 
    829     int newState = en ? 1 : 0;
    830     int err = 0;
    831     //ALOGV_IF((uint32_t(newState) << what) != (mEnabled & (1 << what)),
    832     //        "sensor state change what=%d", what);
    833 
    834     pthread_mutex_lock(&mMplMutex);
    835     if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
    836         uint32_t sensor_type;
    837         short flags = newState;
    838         mEnabled &= ~(1 << what);
    839         mEnabled |= (uint32_t(flags) << what);
    840         ALOGV_IF(EXTRA_VERBOSE, "mEnabled = %x", mEnabled);
    841         setPowerStates(mEnabled);
    842         pthread_mutex_unlock(&mMplMutex);
    843         if (!newState)
    844             update_delay();
    845         return err;
    846     }
    847     pthread_mutex_unlock(&mMplMutex);
    848     return err;
    849 }
    850 
    851 int MPLSensor::setDelay(int32_t handle, int64_t ns)
    852 {
    853     FUNC_LOG;
    854     ALOGV_IF(EXTRA_VERBOSE,
    855             " setDelay handle: %d rate %d", handle, (int) (ns / 1000000LL));
    856     int what = -1;
    857     switch (handle) {
    858     case ID_A:
    859         what = Accelerometer;
    860         break;
    861     case ID_M:
    862         what = MagneticField;
    863         break;
    864     case ID_O:
    865         what = Orientation;
    866         break;
    867     case ID_GY:
    868         what = Gyro;
    869         break;
    870     case ID_GR:
    871         what = Gravity;
    872         break;
    873     case ID_RV:
    874         what = RotationVector;
    875         break;
    876     case ID_LA:
    877         what = LinearAccel;
    878         break;
    879     default:
    880         what = handle;
    881         break;
    882     }
    883 
    884     if (uint32_t(what) >= numSensors)
    885         return -EINVAL;
    886 
    887     if (ns < 0)
    888         return -EINVAL;
    889 
    890     pthread_mutex_lock(&mMplMutex);
    891     mDelays[what] = ns;
    892     pthread_mutex_unlock(&mMplMutex);
    893     return update_delay();
    894 }
    895 
    896 int MPLSensor::update_delay()
    897 {
    898     FUNC_LOG;
    899     int rv = 0;
    900     bool irq_set[5];
    901 
    902     pthread_mutex_lock(&mMplMutex);
    903 
    904     if (mEnabled) {
    905         uint64_t wanted = -1LLU;
    906         for (int i = 0; i < numSensors; i++) {
    907             if (mEnabled & (1 << i)) {
    908                 uint64_t ns = mDelays[i];
    909                 wanted = wanted < ns ? wanted : ns;
    910             }
    911         }
    912 
    913         //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns
    914         if (wanted < 10000000LLU) {
    915             wanted = 10000000LLU;
    916         }
    917 
    918         int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1
    919                                                                          : 0); //mpu fifo rate is in increments of 5ms
    920         if (rate == 0) //KLP disallow fifo rate 0
    921             rate = 1;
    922 
    923         if (rate != mCurFifoRate) {
    924             //ALOGD("set fifo rate: %d %llu", rate, wanted);
    925             inv_error_t res; // = inv_dmp_stop();
    926             res = inv_set_fifo_rate(rate);
    927             ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate");
    928 
    929             //res = inv_dmp_start();
    930             //ALOGE_IF(res != INV_SUCCESS, "error re-starting DMP");
    931 
    932             mCurFifoRate = rate;
    933             rv = (res == INV_SUCCESS);
    934         }
    935 
    936         if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) {
    937             if (mUseTimerirq) {
    938                 ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
    939                 clearIrqData(irq_set);
    940                 if (inv_get_dl_config()->requested_sensors
    941                         == INV_THREE_AXIS_COMPASS) {
    942                     ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START,
    943                           (unsigned long) (wanted / 1000000LLU));
    944                     ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d",
    945                             (int) (wanted / 1000000LLU));
    946                 } else {
    947                     ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START,
    948                           (unsigned long) inv_get_sample_step_size_ms());
    949                     ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d",
    950                             (int) inv_get_sample_step_size_ms());
    951                 }
    952             }
    953         }
    954 
    955     }
    956     pthread_mutex_unlock(&mMplMutex);
    957     return rv;
    958 }
    959 
    960 /* return the current time in nanoseconds */
    961 int64_t MPLSensor::now_ns(void)
    962 {
    963     //FUNC_LOG;
    964     struct timespec ts;
    965 
    966     clock_gettime(CLOCK_MONOTONIC, &ts);
    967     //ALOGV("Time %lld", (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec);
    968     return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
    969 }
    970 
    971 int MPLSensor::readEvents(sensors_event_t* data, int count)
    972 {
    973     //VFUNC_LOG;
    974     int i;
    975     bool irq_set[5] = { false, false, false, false, false };
    976     inv_error_t rv;
    977     if (count < 1)
    978         return -EINVAL;
    979     int numEventReceived = 0;
    980 
    981     clearIrqData(irq_set);
    982 
    983     pthread_mutex_lock(&mMplMutex);
    984     if (mDmpStarted) {
    985         //ALOGV_IF(EXTRA_VERBOSE, "Update Data");
    986         rv = inv_update_data();
    987         ALOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv);
    988     }
    989 
    990     else {
    991         //probably just one extra read after shutting down
    992         ALOGV_IF(EXTRA_VERBOSE,
    993                 "MPLSensor::readEvents called, but there's nothing to do.");
    994     }
    995 
    996     pthread_mutex_unlock(&mMplMutex);
    997 
    998     if (!mNewData) {
    999         ALOGV_IF(EXTRA_VERBOSE, "no new data");
   1000         return 0;
   1001     }
   1002     mNewData = 0;
   1003 
   1004     /* google timestamp */
   1005     pthread_mutex_lock(&mMplMutex);
   1006     for (int i = 0; i < numSensors; i++) {
   1007         if (mEnabled & (1 << i)) {
   1008             CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i,
   1009                                               &mPendingMask, i);
   1010 	    mPendingEvents[i].timestamp = irq_timestamp;
   1011         }
   1012     }
   1013 
   1014     for (int j = 0; count && mPendingMask && j < numSensors; j++) {
   1015         if (mPendingMask & (1 << j)) {
   1016             mPendingMask &= ~(1 << j);
   1017             if (mEnabled & (1 << j)) {
   1018                 *data++ = mPendingEvents[j];
   1019                 count--;
   1020                 numEventReceived++;
   1021             }
   1022         }
   1023 
   1024     }
   1025 
   1026     pthread_mutex_unlock(&mMplMutex);
   1027     return numEventReceived;
   1028 }
   1029 
   1030 int MPLSensor::getFd() const
   1031 {
   1032     //ALOGV("MPLSensor::getFd returning %d", data_fd);
   1033     return data_fd;
   1034 }
   1035 
   1036 int MPLSensor::getAccelFd() const
   1037 {
   1038     //ALOGV("MPLSensor::getAccelFd returning %d", accel_fd);
   1039     return accel_fd;
   1040 }
   1041 
   1042 int MPLSensor::getTimerFd() const
   1043 {
   1044     //ALOGV("MPLSensor::getTimerFd returning %d", timer_fd);
   1045     return timer_fd;
   1046 }
   1047 
   1048 int MPLSensor::getPowerFd() const
   1049 {
   1050     int hdl = (uintptr_t) inv_get_serial_handle();
   1051     //ALOGV("MPLSensor::getPowerFd returning %d", hdl);
   1052     return hdl;
   1053 }
   1054 
   1055 int MPLSensor::getPollTime()
   1056 {
   1057     return mPollTime;
   1058 }
   1059 
   1060 bool MPLSensor::hasPendingEvents() const
   1061 {
   1062     //if we are using the polling workaround, force the main loop to check for data every time
   1063     return (mPollTime != -1);
   1064 }
   1065 
   1066 void MPLSensor::handlePowerEvent()
   1067 {
   1068     VFUNC_LOG;
   1069     mpuirq_data irqd;
   1070 
   1071     int fd = (uintptr_t) inv_get_serial_handle();
   1072     read(fd, &irqd, sizeof(irqd));
   1073 
   1074     if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) {
   1075         //going to sleep
   1076         sleepEvent();
   1077     } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) {
   1078         //waking up
   1079         wakeEvent();
   1080     }
   1081 
   1082     ioctl(fd, MPU_PM_EVENT_HANDLED, 0);
   1083 }
   1084 
   1085 void MPLSensor::sleepEvent()
   1086 {
   1087     VFUNC_LOG;
   1088     pthread_mutex_lock(&mMplMutex);
   1089     if (mEnabled != 0) {
   1090         mForceSleep = true;
   1091         mOldEnabledMask = mEnabled;
   1092         setPowerStates(0);
   1093     }
   1094     pthread_mutex_unlock(&mMplMutex);
   1095 }
   1096 
   1097 void MPLSensor::wakeEvent()
   1098 {
   1099     VFUNC_LOG;
   1100     pthread_mutex_lock(&mMplMutex);
   1101     if (mForceSleep) {
   1102         setPowerStates((mOldEnabledMask | mEnabled));
   1103     }
   1104     mForceSleep = false;
   1105     pthread_mutex_unlock(&mMplMutex);
   1106 }
   1107 
   1108 /** fill in the sensor list based on which sensors are configured.
   1109  *  return the number of configured sensors.
   1110  *  parameter list must point to a memory region of at least 7*sizeof(sensor_t)
   1111  *  parameter len gives the length of the buffer pointed to by list
   1112  */
   1113 
   1114 int MPLSensor::populateSensorList(struct sensor_t *list, int len)
   1115 {
   1116     int numsensors;
   1117 
   1118     if(len < 7*sizeof(sensor_t)) {
   1119         ALOGE("sensor list too small, not populating.");
   1120         return 0;
   1121     }
   1122 
   1123     /* fill in the base values */
   1124     memcpy(list, sSensorList, sizeof (struct sensor_t) * 7);
   1125 
   1126     /* first add gyro, accel and compass to the list */
   1127 
   1128     /* fill in accel values                          */
   1129     unsigned short accelId = inv_get_accel_id();
   1130     if(accelId == 0)
   1131     {
   1132 	ALOGE("Can not get accel id");
   1133     }
   1134     fillAccel(accelId, list);
   1135 
   1136     /* fill in compass values                        */
   1137     unsigned short compassId = inv_get_compass_id();
   1138     if(compassId == 0)
   1139     {
   1140 	ALOGE("Can not get compass id");
   1141     }
   1142     fillCompass(compassId, list);
   1143 
   1144     /* fill in gyro values                           */
   1145     fillGyro(MPU_NAME, list);
   1146 
   1147     if(mNineAxisEnabled)
   1148     {
   1149         numsensors = 7;
   1150         /* all sensors will be added to the list     */
   1151         /* fill in orientation values	             */
   1152         fillOrientation(list);
   1153 
   1154         /* fill in rotation vector values	     */
   1155         fillRV(list);
   1156 
   1157         /* fill in gravity values			     */
   1158         fillGravity(list);
   1159 
   1160         /* fill in Linear accel values            */
   1161         fillLinearAccel(list);
   1162     } else {
   1163         /* no 9-axis sensors, zero fill that part of the list */
   1164         numsensors = 3;
   1165         memset(list+3, 0, 4*sizeof(struct sensor_t));
   1166     }
   1167 
   1168     return numsensors;
   1169 }
   1170 
   1171 void MPLSensor::fillAccel(unsigned char accel, struct sensor_t *list)
   1172 {
   1173     switch (accel) {
   1174     case ACCEL_ID_LIS331:
   1175         list[Accelerometer].maxRange = ACCEL_LIS331_RANGE;
   1176         list[Accelerometer].resolution = ACCEL_LIS331_RESOLUTION;
   1177         list[Accelerometer].power = ACCEL_LIS331_POWER;
   1178         break;
   1179 
   1180     case ACCEL_ID_LIS3DH:
   1181         list[Accelerometer].maxRange = ACCEL_LIS3DH_RANGE;
   1182         list[Accelerometer].resolution = ACCEL_LIS3DH_RESOLUTION;
   1183         list[Accelerometer].power = ACCEL_LIS3DH_POWER;
   1184         break;
   1185 
   1186     case ACCEL_ID_KXSD9:
   1187         list[Accelerometer].maxRange = ACCEL_KXSD9_RANGE;
   1188         list[Accelerometer].resolution = ACCEL_KXSD9_RESOLUTION;
   1189         list[Accelerometer].power = ACCEL_KXSD9_POWER;
   1190         break;
   1191 
   1192     case ACCEL_ID_KXTF9:
   1193         list[Accelerometer].maxRange = ACCEL_KXTF9_RANGE;
   1194         list[Accelerometer].resolution = ACCEL_KXTF9_RESOLUTION;
   1195         list[Accelerometer].power = ACCEL_KXTF9_POWER;
   1196         break;
   1197 
   1198     case ACCEL_ID_BMA150:
   1199         list[Accelerometer].maxRange = ACCEL_BMA150_RANGE;
   1200         list[Accelerometer].resolution = ACCEL_BMA150_RESOLUTION;
   1201         list[Accelerometer].power = ACCEL_BMA150_POWER;
   1202         break;
   1203 
   1204     case ACCEL_ID_BMA222:
   1205         list[Accelerometer].maxRange = ACCEL_BMA222_RANGE;
   1206         list[Accelerometer].resolution = ACCEL_BMA222_RESOLUTION;
   1207         list[Accelerometer].power = ACCEL_BMA222_POWER;
   1208         break;
   1209 
   1210     case ACCEL_ID_BMA250:
   1211         list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
   1212         list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
   1213         list[Accelerometer].power = ACCEL_BMA250_POWER;
   1214         break;
   1215 
   1216     case ACCEL_ID_ADXL34X:
   1217         list[Accelerometer].maxRange = ACCEL_ADXL34X_RANGE;
   1218         list[Accelerometer].resolution = ACCEL_ADXL34X_RESOLUTION;
   1219         list[Accelerometer].power = ACCEL_ADXL34X_POWER;
   1220         break;
   1221 
   1222     case ACCEL_ID_MMA8450:
   1223         list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
   1224         list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
   1225         list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
   1226         break;
   1227 
   1228     case ACCEL_ID_MMA845X:
   1229         list[Accelerometer].maxRange = ACCEL_MMA845X_RANGE;
   1230         list[Accelerometer].resolution = ACCEL_MMA845X_RESOLUTION;
   1231         list[Accelerometer].power = ACCEL_MMA845X_POWER;
   1232         break;
   1233 
   1234     case ACCEL_ID_MPU6050:
   1235         list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
   1236         list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
   1237         list[Accelerometer].power = ACCEL_MPU6050_POWER;
   1238         break;
   1239     default:
   1240         ALOGE("unknown accel id -- accel params will be wrong.");
   1241         break;
   1242     }
   1243 }
   1244 
   1245 void MPLSensor::fillCompass(unsigned char compass, struct sensor_t *list)
   1246 {
   1247     switch (compass) {
   1248     case COMPASS_ID_AK8975:
   1249         list[MagneticField].maxRange = COMPASS_AKM8975_RANGE;
   1250         list[MagneticField].resolution = COMPASS_AKM8975_RESOLUTION;
   1251         list[MagneticField].power = COMPASS_AKM8975_POWER;
   1252         break;
   1253     case COMPASS_ID_AMI30X:
   1254         list[MagneticField].maxRange = COMPASS_AMI30X_RANGE;
   1255         list[MagneticField].resolution = COMPASS_AMI30X_RESOLUTION;
   1256         list[MagneticField].power = COMPASS_AMI30X_POWER;
   1257         break;
   1258     case COMPASS_ID_AMI306:
   1259         list[MagneticField].maxRange = COMPASS_AMI306_RANGE;
   1260         list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION;
   1261         list[MagneticField].power = COMPASS_AMI306_POWER;
   1262         break;
   1263     case COMPASS_ID_YAS529:
   1264         list[MagneticField].maxRange = COMPASS_YAS529_RANGE;
   1265         list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION;
   1266         list[MagneticField].power = COMPASS_AMI306_POWER;
   1267         break;
   1268     case COMPASS_ID_YAS530:
   1269         list[MagneticField].maxRange = COMPASS_YAS530_RANGE;
   1270         list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION;
   1271         list[MagneticField].power = COMPASS_YAS530_POWER;
   1272         break;
   1273     case COMPASS_ID_HMC5883:
   1274         list[MagneticField].maxRange = COMPASS_HMC5883_RANGE;
   1275         list[MagneticField].resolution = COMPASS_HMC5883_RESOLUTION;
   1276         list[MagneticField].power = COMPASS_HMC5883_POWER;
   1277         break;
   1278     case COMPASS_ID_MMC314X:
   1279         list[MagneticField].maxRange = COMPASS_MMC314X_RANGE;
   1280         list[MagneticField].resolution = COMPASS_MMC314X_RESOLUTION;
   1281         list[MagneticField].power = COMPASS_MMC314X_POWER;
   1282         break;
   1283     case COMPASS_ID_HSCDTD002B:
   1284         list[MagneticField].maxRange = COMPASS_HSCDTD002B_RANGE;
   1285         list[MagneticField].resolution = COMPASS_HSCDTD002B_RESOLUTION;
   1286         list[MagneticField].power = COMPASS_HSCDTD002B_POWER;
   1287         break;
   1288     case COMPASS_ID_HSCDTD004A:
   1289         list[MagneticField].maxRange = COMPASS_HSCDTD004A_RANGE;
   1290         list[MagneticField].resolution = COMPASS_HSCDTD004A_RESOLUTION;
   1291         list[MagneticField].power = COMPASS_HSCDTD004A_POWER;
   1292         break;
   1293     default:
   1294         ALOGE("unknown compass id -- compass parameters will be wrong");
   1295     }
   1296 }
   1297 
   1298 void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
   1299 {
   1300     if ((gyro != NULL) && (strcmp(gyro, "mpu3050") == 0)) {
   1301         list[Gyro].maxRange = GYRO_MPU3050_RANGE;
   1302         list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
   1303         list[Gyro].power = GYRO_MPU3050_POWER;
   1304     } else {
   1305         list[Gyro].maxRange = GYRO_MPU6050_RANGE;
   1306         list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
   1307         list[Gyro].power = GYRO_MPU6050_POWER;
   1308     }
   1309     return;
   1310 }
   1311 
   1312 
   1313 /* fillRV depends on values of accel and compass in the list	*/
   1314 void MPLSensor::fillRV(struct sensor_t *list)
   1315 {
   1316     /* compute power on the fly */
   1317     list[RotationVector].power = list[Gyro].power + list[Accelerometer].power
   1318             + list[MagneticField].power;
   1319     list[RotationVector].resolution = .00001;
   1320     list[RotationVector].maxRange = 1.0;
   1321     return;
   1322 }
   1323 
   1324 void MPLSensor::fillOrientation(struct sensor_t *list)
   1325 {
   1326     list[Orientation].power = list[Gyro].power + list[Accelerometer].power
   1327             + list[MagneticField].power;
   1328     list[Orientation].resolution = .00001;
   1329     list[Orientation].maxRange = 360.0;
   1330     return;
   1331 }
   1332 
   1333 void MPLSensor::fillGravity( struct sensor_t *list)
   1334 {
   1335     list[Gravity].power = list[Gyro].power + list[Accelerometer].power
   1336             + list[MagneticField].power;
   1337     list[Gravity].resolution = .00001;
   1338     list[Gravity].maxRange = 9.81;
   1339     return;
   1340 }
   1341 
   1342 void MPLSensor::fillLinearAccel(struct sensor_t *list)
   1343 {
   1344     list[Gravity].power = list[Gyro].power + list[Accelerometer].power
   1345             + list[MagneticField].power;
   1346     list[Gravity].resolution = list[Accelerometer].resolution;
   1347     list[Gravity].maxRange = list[Accelerometer].maxRange;
   1348     return;
   1349 }
   1350