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
mot_cb_wrapper(uint16_t val)109 void mot_cb_wrapper(uint16_t val)
110 {
111 if (gMPLSensor) {
112 gMPLSensor->cbOnMotion(val);
113 }
114 }
115
procData_cb_wrapper()116 void procData_cb_wrapper()
117 {
118 if(gMPLSensor) {
119 gMPLSensor->cbProcData();
120 }
121 }
122
123 } //end of extern C
124
setCallbackObject(MPLSensor * gbpt)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
MPLSensor()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
~MPLSensor()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 */
clearIrqData(bool * irq_set)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. */
setPowerStates(int enabled_sensors)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 */
initMPL()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 */
setupFIFO()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 */
setupCallbacks()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 */
cbOnMotion(uint16_t val)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
cbProcData()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
gyroHandler(sensors_event_t * s,uint32_t * pending_mask,int index)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
accelHandler(sensors_event_t * s,uint32_t * pending_mask,int index)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
estimateCompassAccuracy()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
compassHandler(sensors_event_t * s,uint32_t * pending_mask,int index)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
rvHandler(sensors_event_t * s,uint32_t * pending_mask,int index)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
laHandler(sensors_event_t * s,uint32_t * pending_mask,int index)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
gravHandler(sensors_event_t * s,uint32_t * pending_mask,int index)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
calcOrientationSensor(float * R,float * values)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
orienHandler(sensors_event_t * s,uint32_t * pending_mask,int index)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
enable(int32_t handle,int en)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
setDelay(int32_t handle,int64_t ns)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
update_delay()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 */
now_ns(void)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
readEvents(sensors_event_t * data,int count)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
getFd() const1030 int MPLSensor::getFd() const
1031 {
1032 //ALOGV("MPLSensor::getFd returning %d", data_fd);
1033 return data_fd;
1034 }
1035
getAccelFd() const1036 int MPLSensor::getAccelFd() const
1037 {
1038 //ALOGV("MPLSensor::getAccelFd returning %d", accel_fd);
1039 return accel_fd;
1040 }
1041
getTimerFd() const1042 int MPLSensor::getTimerFd() const
1043 {
1044 //ALOGV("MPLSensor::getTimerFd returning %d", timer_fd);
1045 return timer_fd;
1046 }
1047
getPowerFd() const1048 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
getPollTime()1055 int MPLSensor::getPollTime()
1056 {
1057 return mPollTime;
1058 }
1059
hasPendingEvents() const1060 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
handlePowerEvent()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
sleepEvent()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
wakeEvent()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
populateSensorList(struct sensor_t * list,int len)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
fillAccel(unsigned char accel,struct sensor_t * list)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
fillCompass(unsigned char compass,struct sensor_t * list)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
fillGyro(const char * gyro,struct sensor_t * list)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 */
fillRV(struct sensor_t * 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
fillOrientation(struct sensor_t * list)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
fillGravity(struct sensor_t * list)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
fillLinearAccel(struct sensor_t * list)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