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