1 /*
2 * Copyright (C) 2012 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 in the MPLSensor.h header file
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 <sys/syscall.h>
30 #include <dlfcn.h>
31 #include <pthread.h>
32 #include <cutils/log.h>
33 #include <utils/KeyedVector.h>
34 #include <utils/String8.h>
35 #include <string.h>
36 #include <linux/input.h>
37 #include <utils/Atomic.h>
38
39 #include "MPLSensor.h"
40 #include "MPLSupport.h"
41 #include "sensor_params.h"
42 #include "local_log_def.h"
43
44 #include "invensense.h"
45 #include "invensense_adv.h"
46 #include "ml_stored_data.h"
47 #include "ml_load_dmp.h"
48 #include "ml_sysfs_helper.h"
49
50 // #define TESTING
51
52 #ifdef THIRD_PARTY_ACCEL
53 # warning "Third party accel"
54 # define USE_THIRD_PARTY_ACCEL (1)
55 #else
56 # define USE_THIRD_PARTY_ACCEL (0)
57 #endif
58
59 #define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
60
61 /******************************************************************************/
62 /* MPL interface misc. */
63 /******************************************************************************/
64 static int hertz_request = 200;
65
66 #define DEFAULT_MPL_GYRO_RATE (20000L) //us
67 #define DEFAULT_MPL_COMPASS_RATE (20000L) //us
68
69 #define DEFAULT_HW_GYRO_RATE (100) //Hz
70 #define DEFAULT_HW_ACCEL_RATE (20) //ms
71 #define DEFAULT_HW_COMPASS_RATE (20000000L) //ns
72 #define DEFAULT_HW_AKMD_COMPASS_RATE (200000000L) //ns
73
74 /* convert ns to hardware units */
75 #define HW_GYRO_RATE_NS (1000000000LL / rate_request) // to Hz
76 #define HW_ACCEL_RATE_NS (rate_request / (1000000L)) // to ms
77 #define HW_COMPASS_RATE_NS (rate_request) // to ns
78
79 /* convert Hz to hardware units */
80 #define HW_GYRO_RATE_HZ (hertz_request)
81 #define HW_ACCEL_RATE_HZ (1000 / hertz_request)
82 #define HW_COMPASS_RATE_HZ (1000000000LL / hertz_request)
83
84 #define RATE_200HZ 5000000LL
85 #define RATE_15HZ 66667000LL
86 #define RATE_5HZ 200000000LL
87
88 static struct sensor_t sSensorList[] =
89 {
90 {"MPL Gyroscope", "Invensense", 1,
91 SENSORS_GYROSCOPE_HANDLE,
92 SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
93 {"MPL Raw Gyroscope", "Invensense", 1,
94 SENSORS_RAW_GYROSCOPE_HANDLE,
95 SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
96 {"MPL Accelerometer", "Invensense", 1,
97 SENSORS_ACCELERATION_HANDLE,
98 SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
99 {"MPL Magnetic Field", "Invensense", 1,
100 SENSORS_MAGNETIC_FIELD_HANDLE,
101 SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
102 {"MPL Orientation", "Invensense", 1,
103 SENSORS_ORIENTATION_HANDLE,
104 SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, {}},
105 {"MPL Rotation Vector", "Invensense", 1,
106 SENSORS_ROTATION_VECTOR_HANDLE,
107 SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
108 {"MPL Linear Acceleration", "Invensense", 1,
109 SENSORS_LINEAR_ACCEL_HANDLE,
110 SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
111 {"MPL Gravity", "Invensense", 1,
112 SENSORS_GRAVITY_HANDLE,
113 SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}},
114
115 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
116 {"MPL Screen Orientation", "Invensense ", 1,
117 SENSORS_SCREEN_ORIENTATION_HANDLE,
118 SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, 0, 0, 0, 0, {}},
119 #endif
120 };
121
122 MPLSensor *MPLSensor::gMPLSensor = NULL;
123
124 extern "C" {
procData_cb_wrapper()125 void procData_cb_wrapper()
126 {
127 if(MPLSensor::gMPLSensor) {
128 MPLSensor::gMPLSensor->cbProcData();
129 }
130 }
131
setCallbackObject(MPLSensor * gbpt)132 void setCallbackObject(MPLSensor* gbpt)
133 {
134 MPLSensor::gMPLSensor = gbpt;
135 }
136
getCallbackObject()137 MPLSensor* getCallbackObject() {
138 return MPLSensor::gMPLSensor;
139 }
140
141 } // end of extern C
142
143 #ifdef INV_PLAYBACK_DBG
144 static FILE *logfile = NULL;
145 #endif
146
147 pthread_mutex_t GlobalHalMutex = PTHREAD_MUTEX_INITIALIZER;
148
149 /*******************************************************************************
150 * MPLSensor class implementation
151 ******************************************************************************/
152
MPLSensor(CompassSensor * compass,int (* m_pt2AccelCalLoadFunc)(long *))153 MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
154 : SensorBase(NULL, NULL),
155 mNewData(0),
156 mMasterSensorMask(INV_ALL_SENSORS),
157 mLocalSensorMask(0),
158 mPollTime(-1),
159 mHaveGoodMpuCal(0),
160 mGyroAccuracy(0),
161 mAccelAccuracy(0),
162 mCompassAccuracy(0),
163 mSampleCount(0),
164 dmp_orient_fd(-1),
165 mDmpOrientationEnabled(0),
166 mEnabled(0),
167 mOldEnabledMask(0),
168 mAccelInputReader(4),
169 mGyroInputReader(32),
170 mTempScale(0),
171 mTempOffset(0),
172 mTempCurrentTime(0),
173 mAccelScale(2),
174 mPendingMask(0),
175 mSensorMask(0),
176 mFeatureActiveMask(0) {
177 VFUNC_LOG;
178
179 inv_error_t rv;
180 int i, fd;
181 char *port = NULL;
182 char *ver_str;
183 unsigned long mSensorMask;
184 int res;
185 FILE *fptr;
186
187 mCompassSensor = compass;
188
189 LOGV_IF(EXTRA_VERBOSE,
190 "HAL:MPLSensor constructor : numSensors = %d", numSensors);
191
192 pthread_mutex_init(&mMplMutex, NULL);
193 pthread_mutex_init(&mHALMutex, NULL);
194 memset(mGyroOrientation, 0, sizeof(mGyroOrientation));
195 memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
196
197 #ifdef INV_PLAYBACK_DBG
198 LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
199 logfile = fopen("/data/playback.bin", "wb");
200 if (logfile)
201 inv_turn_on_data_logging(logfile);
202 #endif
203
204 /* setup sysfs paths */
205 inv_init_sysfs_attributes();
206
207 /* get chip name */
208 if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
209 LOGE("HAL:ERR- Failed to get chip ID\n");
210 } else {
211 LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID);
212 }
213
214 enable_iio_sysfs();
215
216 /* turn on power state */
217 onPower(1);
218
219 /* reset driver master enable */
220 masterEnable(0);
221
222 if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
223 /* Load DMP image if capable, ie. MPU6xxx/9xxx */
224 loadDMP();
225 }
226
227 /* open temperature fd for temp comp */
228 LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
229 gyro_temperature_fd = open(mpu.temperature, O_RDONLY);
230 if (gyro_temperature_fd == -1) {
231 LOGE("HAL:could not open temperature node");
232 } else {
233 LOGV_IF(EXTRA_VERBOSE,
234 "HAL:temperature_fd opened: %s", mpu.temperature);
235 }
236
237 /* read accel FSR to calcuate accel scale later */
238 if (!USE_THIRD_PARTY_ACCEL) {
239 char buf[3];
240 int count = 0;
241 LOGV_IF(SYSFS_VERBOSE,
242 "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());
243
244 fd = open(mpu.accel_fsr, O_RDONLY);
245 if(fd < 0) {
246 LOGE("HAL:Error opening accel FSR");
247 } else {
248 memset(buf, 0, sizeof(buf));
249 count = read_attribute_sensor(fd, buf, sizeof(buf));
250 if(count < 1) {
251 LOGE("HAL:Error reading accel FSR");
252 } else {
253 count = sscanf(buf, "%d", &mAccelScale);
254 if(count)
255 LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
256 }
257 close(fd);
258 }
259 }
260
261 /* initialize sensor data */
262 memset(mPendingEvents, 0, sizeof(mPendingEvents));
263
264 mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
265 mPendingEvents[RotationVector].sensor = ID_RV;
266 mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
267
268 mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
269 mPendingEvents[LinearAccel].sensor = ID_LA;
270 mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
271
272 mPendingEvents[Gravity].version = sizeof(sensors_event_t);
273 mPendingEvents[Gravity].sensor = ID_GR;
274 mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
275
276 mPendingEvents[Gyro].version = sizeof(sensors_event_t);
277 mPendingEvents[Gyro].sensor = ID_GY;
278 mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
279
280 mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
281 mPendingEvents[RawGyro].sensor = ID_RG;
282 mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE;
283
284 mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
285 mPendingEvents[Accelerometer].sensor = ID_A;
286 mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
287
288 /* Invensense compass calibration */
289 mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
290 mPendingEvents[MagneticField].sensor = ID_M;
291 mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
292 mPendingEvents[MagneticField].magnetic.status =
293 SENSOR_STATUS_ACCURACY_HIGH;
294
295 mPendingEvents[Orientation].version = sizeof(sensors_event_t);
296 mPendingEvents[Orientation].sensor = ID_O;
297 mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
298 mPendingEvents[Orientation].orientation.status
299 = SENSOR_STATUS_ACCURACY_HIGH;
300
301 mHandlers[RotationVector] = &MPLSensor::rvHandler;
302 mHandlers[LinearAccel] = &MPLSensor::laHandler;
303 mHandlers[Gravity] = &MPLSensor::gravHandler;
304 mHandlers[Gyro] = &MPLSensor::gyroHandler;
305 mHandlers[RawGyro] = &MPLSensor::rawGyroHandler;
306 mHandlers[Accelerometer] = &MPLSensor::accelHandler;
307 mHandlers[MagneticField] = &MPLSensor::compassHandler;
308 mHandlers[Orientation] = &MPLSensor::orienHandler;
309
310 for (int i = 0; i < numSensors; i++) {
311 mDelays[i] = 0;
312 }
313
314 (void)inv_get_version(&ver_str);
315 LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str);
316
317 /* setup MPL */
318 inv_constructor_init();
319
320 /* load calibration file from /data/inv_cal_data.bin */
321 rv = inv_load_calibration();
322 if(rv == INV_SUCCESS)
323 LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
324 else
325 LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
326
327 /* Takes external Accel Calibration Load Method */
328 if( m_pt2AccelCalLoadFunc != NULL)
329 {
330 long accel_offset[3];
331 long tmp_offset[3];
332 int result = m_pt2AccelCalLoadFunc(accel_offset);
333 if(result)
334 LOGW("HAL:Vendor accelerometer calibration file load failed %d\n", result);
335 else
336 {
337 LOGW("HAL:Vendor accelerometer calibration file successfully loaded");
338 inv_get_accel_bias(tmp_offset, NULL);
339 LOGV_IF(PROCESS_VERBOSE, "HAL:Original accel offset, %ld, %ld, %ld\n",
340 tmp_offset[0], tmp_offset[1], tmp_offset[2]);
341 inv_set_accel_bias(accel_offset, mAccelAccuracy);
342 inv_get_accel_bias(tmp_offset, NULL);
343 LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n",
344 tmp_offset[0], tmp_offset[1], tmp_offset[2]);
345 }
346 }
347 /* End of Accel Calibration Load Method */
348
349 inv_set_device_properties();
350
351 /* disable driver master enable the first sensor goes on */
352 masterEnable(0);
353 enableGyro(0);
354 enableAccel(0);
355 enableCompass(0);
356
357 if (isLowPowerQuatEnabled()) {
358 enableLPQuaternion(0);
359 }
360
361 onPower(0);
362
363 if (isDmpDisplayOrientationOn()) {
364 enableDmpOrientation(!isDmpScreenAutoRotationEnabled());
365 }
366
367 }
368
enable_iio_sysfs()369 void MPLSensor::enable_iio_sysfs()
370 {
371 VFUNC_LOG;
372
373 char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
374 FILE *tempFp = NULL;
375
376 /* ignore failures */
377 write_sysfs_int(mpu.in_timestamp_en, 1);
378
379 LOGV_IF(SYSFS_VERBOSE,
380 "HAL:sysfs:cat %s (%lld)",
381 mpu.trigger_name, getTimestamp());
382 tempFp = fopen(mpu.trigger_name, "r");
383 if (tempFp == NULL) {
384 LOGE("HAL:could not open trigger name");
385 } else {
386 if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
387 LOGE("HAL:could not read trigger name");
388 }
389 fclose(tempFp);
390 }
391
392 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)",
393 iio_trigger_name, mpu.current_trigger, getTimestamp());
394 tempFp = fopen(mpu.current_trigger, "w");
395 if (tempFp == NULL) {
396 LOGE("HAL:could not open current trigger");
397 } else {
398 if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) {
399 LOGE("HAL:could not write current trigger %s err=%d", iio_trigger_name, errno);
400 }
401 }
402
403 write_sysfs_int(mpu.buffer_length, IIO_BUFFER_LENGTH);
404
405 if (inv_get_iio_device_node(iio_device_node) < 0) {
406 LOGE("HAL:could retrive the iio device node");
407 }
408 iio_fd = open(iio_device_node, O_RDONLY);
409 if (iio_fd < 0) {
410 LOGE("HAL:could not open iio device node");
411 } else {
412 LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd (%s) opened: %d", iio_device_node, iio_fd);
413 }
414 }
415
inv_constructor_init()416 int MPLSensor::inv_constructor_init()
417 {
418 VFUNC_LOG;
419
420 inv_error_t result = inv_init_mpl();
421 if (result) {
422 LOGE("HAL:inv_init_mpl() failed");
423 return result;
424 }
425 result = inv_constructor_default_enable();
426 result = inv_start_mpl();
427 if (result) {
428 LOGE("HAL:inv_start_mpl() failed");
429 LOG_RESULT_LOCATION(result);
430 return result;
431 }
432
433 return result;
434 }
435
inv_constructor_default_enable()436 int MPLSensor::inv_constructor_default_enable()
437 {
438 VFUNC_LOG;
439
440 inv_error_t result;
441
442 result = inv_enable_quaternion();
443 if (result) {
444 LOGE("HAL:Cannot enable quaternion\n");
445 return result;
446 }
447
448 result = inv_enable_in_use_auto_calibration();
449 if (result) {
450 return result;
451 }
452
453 // result = inv_enable_motion_no_motion();
454 result = inv_enable_fast_nomot();
455 if (result) {
456 return result;
457 }
458
459 result = inv_enable_gyro_tc();
460 if (result) {
461 return result;
462 }
463
464 result = inv_enable_hal_outputs();
465 if (result) {
466 return result;
467 }
468
469 if (!mCompassSensor->providesCalibration()) {
470 /* Invensense compass calibration */
471 LOGV_IF(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled");
472 result = inv_enable_vector_compass_cal();
473 if (result) {
474 LOG_RESULT_LOCATION(result);
475 return result;
476 } else {
477 mFeatureActiveMask |= INV_COMPASS_CAL;
478 }
479
480 // specify MPL's trust weight, used by compass algorithms
481 inv_vector_compass_cal_sensitivity(3);
482
483 result = inv_enable_compass_bias_w_gyro();
484 if (result) {
485 LOG_RESULT_LOCATION(result);
486 return result;
487 }
488
489 result = inv_enable_heading_from_gyro();
490 if (result) {
491 LOG_RESULT_LOCATION(result);
492 return result;
493 }
494
495 result = inv_enable_magnetic_disturbance();
496 if (result) {
497 LOG_RESULT_LOCATION(result);
498 return result;
499 }
500 }
501
502 result = inv_enable_9x_sensor_fusion();
503 if (result) {
504 LOG_RESULT_LOCATION(result);
505 return result;
506 } else {
507 // 9x sensor fusion enables Compass fit
508 mFeatureActiveMask |= INV_COMPASS_FIT;
509 }
510
511 result = inv_enable_no_gyro_fusion();
512 if (result) {
513 LOG_RESULT_LOCATION(result);
514 return result;
515 }
516
517 result = inv_enable_quat_accuracy_monitor();
518 if (result) {
519 LOG_RESULT_LOCATION(result);
520 return result;
521 }
522
523 return result;
524 }
525
526 /* TODO: create function pointers to calculate scale */
inv_set_device_properties()527 void MPLSensor::inv_set_device_properties()
528 {
529 VFUNC_LOG;
530
531 unsigned short orient;
532
533 inv_get_sensors_orientation();
534
535 inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE);
536 inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE);
537
538 /* gyro setup */
539 orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
540 inv_set_gyro_orientation_and_scale(orient, 2000L << 15);
541
542 /* accel setup */
543 orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
544 /* use for third party accel input subsystem driver
545 inv_set_accel_orientation_and_scale(orient, 1LL << 22);
546 */
547 inv_set_accel_orientation_and_scale(orient, mAccelScale << 15);
548
549 /* compass setup */
550 signed char orientMtx[9];
551 mCompassSensor->getOrientationMatrix(orientMtx);
552 orient =
553 inv_orientation_matrix_to_scalar(orientMtx);
554 long sensitivity;
555 sensitivity = mCompassSensor->getSensitivity();
556 inv_set_compass_orientation_and_scale(orient, sensitivity);
557 }
558
loadDMP()559 void MPLSensor::loadDMP()
560 {
561 int res, fd;
562 FILE *fptr;
563
564 if (isMpu3050()) {
565 //DMP support only for MPU6xxx/9xxx currently
566 return;
567 }
568
569 /* load DMP firmware */
570 LOGV_IF(SYSFS_VERBOSE,
571 "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
572 fd = open(mpu.firmware_loaded, O_RDONLY);
573 if(fd < 0) {
574 LOGE("HAL:could not open dmp state");
575 return;
576 }
577 if(inv_read_dmp_state(fd)) {
578 LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
579 return;
580 }
581
582 LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
583 fptr = fopen(mpu.dmp_firmware, "w");
584 if(!fptr) {
585 LOGE("HAL:could open %s for write. %s", mpu.dmp_firmware, strerror(errno));
586 return;
587 }
588 res = inv_load_dmp(fptr);
589 if(res < 0) {
590 LOGE("HAL:load DMP failed");
591 } else {
592 LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
593 }
594 fclose(fptr);
595
596 // onDMP(1); //Can't enable here. See note onDMP()
597 }
598
inv_get_sensors_orientation()599 void MPLSensor::inv_get_sensors_orientation()
600 {
601 FILE *fptr;
602
603 // get gyro orientation
604 LOGV_IF(SYSFS_VERBOSE,
605 "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
606 fptr = fopen(mpu.gyro_orient, "r");
607 if (fptr != NULL) {
608 int om[9];
609 fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
610 &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
611 &om[6], &om[7], &om[8]);
612 fclose(fptr);
613
614 LOGV_IF(EXTRA_VERBOSE,
615 "HAL:gyro mounting matrix: "
616 "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
617 om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
618
619 mGyroOrientation[0] = om[0];
620 mGyroOrientation[1] = om[1];
621 mGyroOrientation[2] = om[2];
622 mGyroOrientation[3] = om[3];
623 mGyroOrientation[4] = om[4];
624 mGyroOrientation[5] = om[5];
625 mGyroOrientation[6] = om[6];
626 mGyroOrientation[7] = om[7];
627 mGyroOrientation[8] = om[8];
628 } else {
629 LOGE("HAL:Couldn't read gyro mounting matrix");
630 }
631
632 // get accel orientation
633 LOGV_IF(SYSFS_VERBOSE,
634 "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
635 fptr = fopen(mpu.accel_orient, "r");
636 if (fptr != NULL) {
637 int om[9];
638 fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
639 &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
640 &om[6], &om[7], &om[8]);
641 fclose(fptr);
642
643 LOGV_IF(EXTRA_VERBOSE,
644 "HAL:accel mounting matrix: "
645 "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
646 om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
647
648 mAccelOrientation[0] = om[0];
649 mAccelOrientation[1] = om[1];
650 mAccelOrientation[2] = om[2];
651 mAccelOrientation[3] = om[3];
652 mAccelOrientation[4] = om[4];
653 mAccelOrientation[5] = om[5];
654 mAccelOrientation[6] = om[6];
655 mAccelOrientation[7] = om[7];
656 mAccelOrientation[8] = om[8];
657 } else {
658 LOGE("HAL:Couldn't read accel mounting matrix");
659 }
660 }
661
~MPLSensor()662 MPLSensor::~MPLSensor()
663 {
664 VFUNC_LOG;
665
666 mCompassSensor = NULL;
667
668 /* Close open fds */
669 if (iio_fd > 0)
670 close(iio_fd);
671 if( accel_fd > 0 )
672 close(accel_fd );
673 if (gyro_temperature_fd > 0)
674 close(gyro_temperature_fd);
675 if (sysfs_names_ptr)
676 free(sysfs_names_ptr);
677
678 if (isDmpDisplayOrientationOn()) {
679 closeDmpOrientFd();
680 }
681
682 /* Turn off Gyro master enable */
683 /* A workaround until driver handles it */
684 /* TODO: Turn off and close all sensors */
685 if(write_sysfs_int(mpu.chip_enable, 0) < 0) {
686 LOGE("HAL:could not disable gyro master enable");
687 }
688
689 #ifdef INV_PLAYBACK_DBG
690 inv_turn_off_data_logging();
691 fclose(logfile);
692 #endif
693 }
694
695 #define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors)
696 #define A_ENABLED ((1 << ID_A) & enabled_sensors)
697 #define M_ENABLED ((1 << ID_M) & enabled_sensors)
698 #define O_ENABLED ((1 << ID_O) & enabled_sensors)
699 #define LA_ENABLED ((1 << ID_LA) & enabled_sensors)
700 #define GR_ENABLED ((1 << ID_GR) & enabled_sensors)
701 #define RV_ENABLED ((1 << ID_RV) & enabled_sensors)
702
703 /* TODO: this step is optional, remove? */
setGyroInitialState()704 int MPLSensor::setGyroInitialState()
705 {
706 VFUNC_LOG;
707
708 int res = 0;
709
710 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
711 HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp());
712 int fd = open(mpu.gyro_fifo_rate, O_RDWR);
713 res = errno;
714 if(fd < 0) {
715 LOGE("HAL:open of %s failed with '%s' (%d)",
716 mpu.gyro_fifo_rate, strerror(res), res);
717 return res;
718 }
719 res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ);
720 if(res < 0) {
721 LOGE("HAL:write_attribute_sensor : error writing %s with %d",
722 mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ);
723 return res;
724 }
725
726 // Setting LPF is deprecated
727
728 return 0;
729 }
730
731 /* this applies to BMA250 Input Subsystem Driver only */
setAccelInitialState()732 int MPLSensor::setAccelInitialState()
733 {
734 VFUNC_LOG;
735
736 struct input_absinfo absinfo_x;
737 struct input_absinfo absinfo_y;
738 struct input_absinfo absinfo_z;
739 float value;
740 if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) &&
741 !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) &&
742 !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) {
743 value = absinfo_x.value;
744 mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X;
745 value = absinfo_y.value;
746 mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y;
747 value = absinfo_z.value;
748 mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z;
749 //mHasPendingEvent = true;
750 }
751 return 0;
752 }
753
onPower(int en)754 int MPLSensor::onPower(int en)
755 {
756 VFUNC_LOG;
757
758 int res;
759
760 int count, curr_power_state;
761
762 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
763 en, mpu.power_state, getTimestamp());
764 res = read_sysfs_int(mpu.power_state, &curr_power_state);
765 if (res < 0) {
766 LOGE("HAL:Error reading power state");
767 // will set power_state anyway
768 curr_power_state = -1;
769 }
770 if (en != curr_power_state) {
771 if((res = write_sysfs_int(mpu.power_state, en)) < 0) {
772 LOGE("HAL:Couldn't write power state");
773 }
774 } else {
775 LOGV_IF(EXTRA_VERBOSE,
776 "HAL:Power state already enable/disable curr=%d new=%d",
777 curr_power_state, en);
778 }
779 return res;
780 }
781
onDMP(int en)782 int MPLSensor::onDMP(int en)
783 {
784 VFUNC_LOG;
785
786 int res = -1;
787 int status;
788
789 //Sequence to enable DMP
790 //1. Turn On power if not already on
791 //2. Load DMP image if not already loaded
792 //3. Either Gyro or Accel must be enabled/configured before next step
793 //4. Enable DMP
794
795 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
796 mpu.firmware_loaded, getTimestamp());
797 res = read_sysfs_int(mpu.firmware_loaded, &status);
798 if (res < 0){
799 LOGE("HAL:ERR can't get firmware_loaded status");
800 return res;
801 }
802 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: %s status=%d", mpu.firmware_loaded, status);
803
804 if (status) {
805 //Write only if curr DMP state <> request
806 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
807 mpu.dmp_on, getTimestamp());
808 if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
809 LOGE("HAL:ERR can't read DMP state");
810 } else if (status != en) {
811 res = write_sysfs_int(mpu.dmp_on, en);
812 //Enable DMP interrupt
813 if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
814 LOGE("HAL:ERR can't en/dis DMP interrupt");
815 }
816 } else {
817 res = 0; //DMP already set as requested
818 }
819 } else {
820 LOGE("HAL:ERR No DMP image");
821 }
822 return res;
823 }
824
checkLPQuaternion(void)825 int MPLSensor::checkLPQuaternion(void)
826 {
827 VFUNC_LOG;
828
829 return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0);
830 }
831
enableLPQuaternion(int en)832 int MPLSensor::enableLPQuaternion(int en)
833 {
834 VFUNC_LOG;
835
836 if (!en) {
837 enableQuaternionData(0);
838 onDMP(0);
839 mFeatureActiveMask &= ~INV_DMP_QUATERNION;
840 LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled");
841 } else {
842 if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {
843 LOGE("HAL:ERR can't enable LP Quaternion");
844 } else {
845 mFeatureActiveMask |= INV_DMP_QUATERNION;
846 LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat enabled");
847 }
848 }
849 return 0;
850 }
851
enableQuaternionData(int en)852 int MPLSensor::enableQuaternionData(int en)
853 {
854 int res = 0;
855 VFUNC_LOG;
856
857 // Enable DMP quaternion
858 res = write_sysfs_int(mpu.quaternion_on, en);
859
860 if (!en) {
861 LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems");
862 } else {
863
864 LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
865 }
866 write_sysfs_int(mpu.in_quat_r_en, en);
867 write_sysfs_int(mpu.in_quat_x_en, en);
868 write_sysfs_int(mpu.in_quat_y_en, en);
869 write_sysfs_int(mpu.in_quat_z_en, en);
870
871 LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
872
873 if (!en) {
874 inv_quaternion_sensor_was_turned_off();
875 }
876
877 return res;
878 }
879
enableTap(int)880 int MPLSensor::enableTap(int /*en*/)
881 {
882 VFUNC_LOG;
883
884 return 0;
885 }
886
enableFlick(int)887 int MPLSensor::enableFlick(int /*en*/)
888 {
889 VFUNC_LOG;
890
891 return 0;
892 }
893
enablePedometer(int)894 int MPLSensor::enablePedometer(int /*en*/)
895 {
896 VFUNC_LOG;
897
898 return 0;
899 }
900
masterEnable(int en)901 int MPLSensor::masterEnable(int en)
902 {
903 VFUNC_LOG;
904 return write_sysfs_int(mpu.chip_enable, en);
905 }
906
enableGyro(int en)907 int MPLSensor::enableGyro(int en)
908 {
909 VFUNC_LOG;
910
911 /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
912 int res;
913
914 /* need to also turn on/off the master enable */
915 res = write_sysfs_int(mpu.gyro_enable, en);
916
917 if (!en) {
918 LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
919 inv_gyro_was_turned_off();
920 } else {
921 write_sysfs_int(mpu.gyro_x_fifo_enable, en);
922 write_sysfs_int(mpu.gyro_y_fifo_enable, en);
923 res = write_sysfs_int(mpu.gyro_z_fifo_enable, en);
924 }
925
926 return res;
927 }
928
enableAccel(int en)929 int MPLSensor::enableAccel(int en)
930 {
931 VFUNC_LOG;
932
933 /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
934 int res;
935
936 /* need to also turn on/off the master enable */
937 res = write_sysfs_int(mpu.accel_enable, en);
938
939 if (!en) {
940 LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
941 inv_accel_was_turned_off();
942 } else {
943 write_sysfs_int(mpu.accel_x_fifo_enable, en);
944 write_sysfs_int(mpu.accel_y_fifo_enable, en);
945 res = write_sysfs_int(mpu.accel_z_fifo_enable, en);
946 }
947
948 return res;
949 }
950
enableCompass(int en)951 int MPLSensor::enableCompass(int en)
952 {
953 VFUNC_LOG;
954
955 int res = mCompassSensor->enable(ID_M, en);
956 if (!en) {
957 LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
958 inv_compass_was_turned_off();
959 }
960 return res;
961 }
962
computeLocalSensorMask(int enabled_sensors)963 void MPLSensor::computeLocalSensorMask(int enabled_sensors)
964 {
965 VFUNC_LOG;
966
967 do {
968 if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
969 LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
970 mLocalSensorMask = ALL_MPL_SENSORS_NP;
971 break;
972 }
973
974 if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
975 /* Invensense compass cal */
976 LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
977 mLocalSensorMask = 0;
978 break;
979 }
980
981 if (GY_ENABLED) {
982 LOGV_IF(ENG_VERBOSE, "G ENABLED");
983 mLocalSensorMask |= INV_THREE_AXIS_GYRO;
984 } else {
985 LOGV_IF(ENG_VERBOSE, "G DISABLED");
986 mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
987 }
988
989 if (A_ENABLED) {
990 LOGV_IF(ENG_VERBOSE, "A ENABLED");
991 mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
992 } else {
993 LOGV_IF(ENG_VERBOSE, "A DISABLED");
994 mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
995 }
996
997 /* Invensense compass calibration */
998 if (M_ENABLED) {
999 LOGV_IF(ENG_VERBOSE, "M ENABLED");
1000 mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
1001 } else {
1002 LOGV_IF(ENG_VERBOSE, "M DISABLED");
1003 mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
1004 }
1005 } while (0);
1006 }
1007
enableOneSensor(int en,const char * name,int (MPLSensor::* enabler)(int))1008 int MPLSensor::enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int)) {
1009 LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - %s %s", en ? "enabled" : "disable", name);
1010 return (this->*enabler)(en);
1011 }
1012
enableSensors(unsigned long sensors,int,uint32_t changed)1013 int MPLSensor::enableSensors(unsigned long sensors, int /*en*/, uint32_t changed) {
1014 VFUNC_LOG;
1015
1016 inv_error_t res = -1;
1017 bool store_cal = false;
1018 bool ext_compass_changed = false;
1019
1020 // Sequence to enable or disable a sensor
1021 // 1. enable Power state
1022 // 2. reset master enable (=0)
1023 // 3. enable or disable a sensor
1024 // 4. set master enable (=1)
1025
1026 pthread_mutex_lock(&GlobalHalMutex);
1027
1028 uint32_t all_changeables = (1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer)
1029 | (1 << MagneticField);
1030 uint32_t all_integrated_changeables = all_changeables;
1031
1032 if (!mCompassSensor->isIntegrated()) {
1033 ext_compass_changed = changed & (1 << MagneticField);
1034 all_integrated_changeables = all_changeables & ~(1 << MagneticField);
1035 }
1036
1037 if (isLowPowerQuatEnabled() || (changed & all_integrated_changeables)) {
1038 /* ensure power state is on */
1039 onPower(1);
1040
1041 /* reset master enable */
1042 res = masterEnable(0);
1043 if(res < 0) {
1044 goto unlock_res;
1045 }
1046 }
1047
1048 LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
1049
1050 if (changed & ((1 << Gyro) | (1 << RawGyro))) {
1051 res = enableOneSensor(sensors & INV_THREE_AXIS_GYRO, "gyro", &MPLSensor::enableGyro);
1052 if(res < 0) {
1053 goto unlock_res;
1054 }
1055 }
1056
1057 if (changed & (1 << Accelerometer)) {
1058 res = enableOneSensor(sensors & INV_THREE_AXIS_ACCEL, "accel", &MPLSensor::enableAccel);
1059 if(res < 0) {
1060 goto unlock_res;
1061 }
1062 }
1063
1064 if (changed & (1 << MagneticField)) {
1065 /* Invensense compass calibration */
1066 res = enableOneSensor(sensors & INV_THREE_AXIS_COMPASS, "compass", &MPLSensor::enableCompass);
1067 if(res < 0) {
1068 goto unlock_res;
1069 }
1070 }
1071
1072 if ( isLowPowerQuatEnabled() ) {
1073 // Enable LP Quat
1074 if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
1075 (1 << LinearAccel) | (1 << Gravity)))) {
1076 if (!(changed & all_integrated_changeables)) {
1077 /* ensure power state is on */
1078 onPower(1);
1079 /* reset master enable */
1080 res = masterEnable(0);
1081 if(res < 0) {
1082 goto unlock_res;
1083 }
1084 }
1085 if (!checkLPQuaternion()) {
1086 enableLPQuaternion(1);
1087 } else {
1088 LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
1089 }
1090 } else if (checkLPQuaternion()) {
1091 enableLPQuaternion(0);
1092 }
1093 }
1094
1095 if (changed & all_integrated_changeables) {
1096 if (sensors &
1097 (INV_THREE_AXIS_GYRO
1098 | INV_THREE_AXIS_ACCEL
1099 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
1100 if ( isLowPowerQuatEnabled() ||
1101 (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) ) {
1102 // disable DMP event interrupt only (w/ data interrupt)
1103 if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
1104 res = -1;
1105 LOGE("HAL:ERR can't disable DMP event interrupt");
1106 goto unlock_res;
1107 }
1108 }
1109
1110 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
1111 // enable DMP
1112 onDMP(1);
1113
1114 res = enableAccel(1);
1115 if(res < 0) {
1116 goto unlock_res;
1117 }
1118
1119 if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
1120 res = turnOffAccelFifo();
1121 }
1122 if(res < 0) {
1123 goto unlock_res;
1124 }
1125 }
1126
1127 res = masterEnable(1);
1128 if(res < 0) {
1129 goto unlock_res;
1130 }
1131 } else { // all sensors idle -> reduce power
1132 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
1133 // enable DMP
1134 onDMP(1);
1135 // enable DMP event interrupt only (no data interrupt)
1136 if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
1137 res = -1;
1138 LOGE("HAL:ERR can't enable DMP event interrupt");
1139 }
1140 res = enableAccel(1);
1141 if(res < 0) {
1142 goto unlock_res;
1143 }
1144 if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
1145 res = turnOffAccelFifo();
1146 }
1147 if(res < 0) {
1148 goto unlock_res;
1149 }
1150 res = masterEnable(1);
1151 if(res < 0) {
1152 goto unlock_res;
1153 }
1154 }
1155 else {
1156 res = onPower(0);
1157 if(res < 0) {
1158 goto unlock_res;
1159 }
1160 }
1161 store_cal = true;
1162 }
1163 } else if (ext_compass_changed &&
1164 !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
1165 | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) {
1166 store_cal = true;
1167 }
1168
1169 if (store_cal || ((changed & all_changeables) != all_changeables)) {
1170 storeCalibration();
1171 }
1172
1173 unlock_res:
1174 pthread_mutex_unlock(&GlobalHalMutex);
1175 return res;
1176 }
1177
1178 /* Store calibration file */
storeCalibration()1179 void MPLSensor::storeCalibration()
1180 {
1181 if(mHaveGoodMpuCal || mAccelAccuracy >= 2 || mCompassAccuracy >= 3) {
1182 int res = inv_store_calibration();
1183 if (res) {
1184 LOGE("HAL:Cannot store calibration on file");
1185 } else {
1186 LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated");
1187 }
1188 }
1189 }
1190
cbProcData()1191 void MPLSensor::cbProcData()
1192 {
1193 mNewData = 1;
1194 mSampleCount++;
1195 LOGV_IF(EXTRA_VERBOSE, "HAL:new data");
1196 }
1197
1198 /* these handlers transform mpl data into one of the Android sensor types */
gyroHandler(sensors_event_t * s)1199 int MPLSensor::gyroHandler(sensors_event_t* s)
1200 {
1201 VHANDLER_LOG;
1202 int8_t status;
1203 int update;
1204 update = inv_get_sensor_type_gyroscope(s->gyro.v, &status, &s->timestamp);
1205 LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
1206 s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
1207 return update;
1208 }
1209
rawGyroHandler(sensors_event_t * s)1210 int MPLSensor::rawGyroHandler(sensors_event_t* s)
1211 {
1212 VHANDLER_LOG;
1213 int8_t status;
1214 int update;
1215 update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &status, &s->timestamp);
1216 LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
1217 s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
1218 return update;
1219 }
1220
accelHandler(sensors_event_t * s)1221 int MPLSensor::accelHandler(sensors_event_t* s)
1222 {
1223 VHANDLER_LOG;
1224 int8_t status;
1225 int update;
1226 update = inv_get_sensor_type_accelerometer(
1227 s->acceleration.v, &status, &s->timestamp);
1228 LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
1229 s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
1230 s->timestamp, update);
1231 mAccelAccuracy = status;
1232 return update;
1233 }
1234
compassHandler(sensors_event_t * s)1235 int MPLSensor::compassHandler(sensors_event_t* s)
1236 {
1237 VHANDLER_LOG;
1238 int update;
1239 update = inv_get_sensor_type_magnetic_field(
1240 s->magnetic.v, &s->magnetic.status, &s->timestamp);
1241 LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
1242 s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update);
1243 mCompassAccuracy = s->magnetic.status;
1244 return update;
1245 }
1246
rvHandler(sensors_event_t * s)1247 int MPLSensor::rvHandler(sensors_event_t* s)
1248 {
1249 // rotation vector does not have an accuracy or status
1250 VHANDLER_LOG;
1251 int8_t status;
1252 int update;
1253 update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp);
1254 LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d",
1255 s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
1256 return update;
1257 }
1258
laHandler(sensors_event_t * s)1259 int MPLSensor::laHandler(sensors_event_t* s)
1260 {
1261 VHANDLER_LOG;
1262 int8_t status;
1263 int update;
1264 update = inv_get_sensor_type_linear_acceleration(
1265 s->gyro.v, &status, &s->timestamp);
1266 LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
1267 s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
1268 return update;
1269 }
1270
gravHandler(sensors_event_t * s)1271 int MPLSensor::gravHandler(sensors_event_t* s)
1272 {
1273 VHANDLER_LOG;
1274 int8_t status;
1275 int update;
1276 update = inv_get_sensor_type_gravity(s->gyro.v, &status, &s->timestamp);
1277 LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
1278 s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
1279 return update;
1280 }
1281
orienHandler(sensors_event_t * s)1282 int MPLSensor::orienHandler(sensors_event_t* s)
1283 {
1284 VHANDLER_LOG;
1285 int update;
1286 update = inv_get_sensor_type_orientation(
1287 s->orientation.v, &s->orientation.status, &s->timestamp);
1288 LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
1289 s->orientation.v[0], s->orientation.v[1], s->orientation.v[2], s->timestamp, update);
1290 return update;
1291 }
1292
enable(int32_t handle,int en)1293 int MPLSensor::enable(int32_t handle, int en)
1294 {
1295 VFUNC_LOG;
1296
1297 android::String8 sname;
1298 int what = -1, err = 0;
1299
1300 switch (handle) {
1301 case ID_SO:
1302 sname = "Screen Orientation";
1303 LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
1304 (mDmpOrientationEnabled? "en": "dis"),
1305 (en? "en" : "dis"));
1306 enableDmpOrientation(en && isDmpDisplayOrientationOn());
1307 /* TODO: stop manually testing/using 0 and 1 instead of
1308 * false and true, but just use 0 and non-0.
1309 * This allows passing 0 and non-0 ints around instead of
1310 * having to convert to 1 and test against 1.
1311 */
1312 mDmpOrientationEnabled = en;
1313 return 0;
1314 case ID_A:
1315 what = Accelerometer;
1316 sname = "Accelerometer";
1317 break;
1318 case ID_M:
1319 what = MagneticField;
1320 sname = "MagneticField";
1321 break;
1322 case ID_O:
1323 what = Orientation;
1324 sname = "Orientation";
1325 break;
1326 case ID_GY:
1327 what = Gyro;
1328 sname = "Gyro";
1329 break;
1330 case ID_RG:
1331 what = RawGyro;
1332 sname = "RawGyro";
1333 break;
1334 case ID_GR:
1335 what = Gravity;
1336 sname = "Gravity";
1337 break;
1338 case ID_RV:
1339 what = RotationVector;
1340 sname = "RotationVector";
1341 break;
1342 case ID_LA:
1343 what = LinearAccel;
1344 sname = "LinearAccel";
1345 break;
1346 default: //this takes care of all the gestures
1347 what = handle;
1348 sname = "Others";
1349 break;
1350 }
1351
1352 if (uint32_t(what) >= numSensors)
1353 return -EINVAL;
1354
1355 int newState = en ? 1 : 0;
1356 unsigned long sen_mask;
1357
1358 LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
1359 ((mEnabled & (1 << what)) ? "en" : "dis"),
1360 ((uint32_t(newState) << what) ? "en" : "dis"));
1361 LOGV_IF(PROCESS_VERBOSE,
1362 "HAL:%s sensor state change what=%d", sname.string(), what);
1363
1364 // pthread_mutex_lock(&mMplMutex);
1365 // pthread_mutex_lock(&mHALMutex);
1366
1367 if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
1368 uint32_t sensor_type;
1369 short flags = newState;
1370 uint32_t lastEnabled = mEnabled, changed = 0;
1371
1372 mEnabled &= ~(1 << what);
1373 mEnabled |= (uint32_t(flags) << what);
1374
1375 LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle);
1376 LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags);
1377 computeLocalSensorMask(mEnabled);
1378 LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
1379 sen_mask = mLocalSensorMask & mMasterSensorMask;
1380 mSensorMask = sen_mask;
1381 LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
1382
1383 switch (what) {
1384 case Gyro:
1385 case RawGyro:
1386 case Accelerometer:
1387 case MagneticField:
1388 if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
1389 (1 << LinearAccel) | (1 << Gravity))) &&
1390 ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
1391 changed |= (1 << what);
1392 }
1393 break;
1394
1395 case Orientation:
1396 case RotationVector:
1397 case LinearAccel:
1398 case Gravity:
1399 if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) |
1400 (1 << LinearAccel) | (1 << Gravity)))) ||
1401 (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
1402 (1 << LinearAccel) | (1 << Gravity))))) {
1403 for (int i = Gyro; i <= MagneticField; i++) {
1404 if (!(mEnabled & (1 << i))) {
1405 changed |= (1 << i);
1406 }
1407 }
1408 }
1409 break;
1410 }
1411 LOGV_IF(PROCESS_VERBOSE, "HAL:changed = %d", changed);
1412 enableSensors(sen_mask, flags, changed);
1413 }
1414
1415 // pthread_mutex_unlock(&mMplMutex);
1416 // pthread_mutex_unlock(&mHALMutex);
1417
1418 #ifdef INV_PLAYBACK_DBG
1419 /* apparently the logging needs to be go through this sequence
1420 to properly flush the log file */
1421 inv_turn_off_data_logging();
1422 fclose(logfile);
1423 logfile = fopen("/data/playback.bin", "ab");
1424 if (logfile)
1425 inv_turn_on_data_logging(logfile);
1426 #endif
1427
1428 return err;
1429 }
1430
setDelay(int32_t handle,int64_t ns)1431 int MPLSensor::setDelay(int32_t handle, int64_t ns)
1432 {
1433 VFUNC_LOG;
1434
1435 android::String8 sname;
1436 int what = -1;
1437
1438 switch (handle) {
1439 case ID_SO:
1440 return update_delay();
1441 case ID_A:
1442 what = Accelerometer;
1443 sname = "Accelerometer";
1444 break;
1445 case ID_M:
1446 what = MagneticField;
1447 sname = "MagneticField";
1448 break;
1449 case ID_O:
1450 what = Orientation;
1451 sname = "Orientation";
1452 break;
1453 case ID_GY:
1454 what = Gyro;
1455 sname = "Gyro";
1456 break;
1457 case ID_RG:
1458 what = RawGyro;
1459 sname = "RawGyro";
1460 break;
1461 case ID_GR:
1462 what = Gravity;
1463 sname = "Gravity";
1464 break;
1465 case ID_RV:
1466 what = RotationVector;
1467 sname = "RotationVector";
1468 break;
1469 case ID_LA:
1470 what = LinearAccel;
1471 sname = "LinearAccel";
1472 break;
1473 default: // this takes care of all the gestures
1474 what = handle;
1475 sname = "Others";
1476 break;
1477 }
1478
1479 #if 0
1480 // skip the 1st call for enalbing sensors called by ICS/JB sensor service
1481 static int counter_delay = 0;
1482 if (!(mEnabled & (1 << what))) {
1483 counter_delay = 0;
1484 } else {
1485 if (++counter_delay == 1) {
1486 return 0;
1487 }
1488 else {
1489 counter_delay = 0;
1490 }
1491 }
1492 #endif
1493
1494 if (uint32_t(what) >= numSensors)
1495 return -EINVAL;
1496
1497 if (ns < 0)
1498 return -EINVAL;
1499
1500 LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
1501
1502 // limit all rates to reasonable ones */
1503 if (ns < 5000000LL) {
1504 ns = 5000000LL;
1505 }
1506
1507 /* store request rate to mDelays array for each sensor */
1508 mDelays[what] = ns;
1509
1510 switch (what) {
1511 case Gyro:
1512 case RawGyro:
1513 case Accelerometer:
1514 for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated();
1515 i++) {
1516 if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
1517 LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
1518 return 0;
1519 }
1520 }
1521 break;
1522
1523 case MagneticField:
1524 if (mCompassSensor->isIntegrated() &&
1525 (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
1526 ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
1527 ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) {
1528 LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel");
1529 return 0;
1530 }
1531 break;
1532
1533 case Orientation:
1534 case RotationVector:
1535 case LinearAccel:
1536 case Gravity:
1537 if (isLowPowerQuatEnabled()) {
1538 LOGV_IF(PROCESS_VERBOSE, "HAL:need to update delay due to LPQ");
1539 break;
1540 }
1541
1542 for (int i = 0; i < numSensors; i++) {
1543 if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
1544 LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
1545 return 0;
1546 }
1547 }
1548 break;
1549 }
1550
1551 // pthread_mutex_lock(&mHALMutex);
1552 int res = update_delay();
1553 // pthread_mutex_unlock(&mHALMutex);
1554 return res;
1555 }
1556
update_delay()1557 int MPLSensor::update_delay() {
1558 VHANDLER_LOG;
1559
1560 int res = 0;
1561 int64_t got;
1562
1563 pthread_mutex_lock(&GlobalHalMutex);
1564 if (mEnabled) {
1565 int64_t wanted = 1000000000;
1566 int64_t wanted_3rd_party_sensor = 1000000000;
1567
1568 // Sequence to change sensor's FIFO rate
1569 // 1. enable Power state
1570 // 2. reset master enable
1571 // 3. Update delay
1572 // 4. set master enable
1573
1574 // ensure power on
1575 onPower(1);
1576
1577 // reset master enable
1578 masterEnable(0);
1579
1580 /* search the minimum delay requested across all enabled sensors */
1581 for (int i = 0; i < numSensors; i++) {
1582 if (mEnabled & (1 << i)) {
1583 int64_t ns = mDelays[i];
1584 wanted = wanted < ns ? wanted : ns;
1585 }
1586 }
1587
1588 // same delay for 3rd party Accel or Compass
1589 wanted_3rd_party_sensor = wanted;
1590
1591 /* mpl rate in us in future maybe different for
1592 gyro vs compass vs accel */
1593 int rateInus = (int)wanted / 1000LL;
1594 int mplGyroRate = rateInus;
1595 int mplAccelRate = rateInus;
1596 int mplCompassRate = rateInus;
1597
1598 LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : "
1599 "%llu ns, mpl rate: %d us, (%.2f Hz)",
1600 wanted, rateInus, 1000000000.f / wanted);
1601
1602 /* set rate in MPL */
1603 /* compass can only do 100Hz max */
1604 inv_set_gyro_sample_rate(mplGyroRate);
1605 inv_set_accel_sample_rate(mplAccelRate);
1606 inv_set_compass_sample_rate(mplCompassRate);
1607
1608 /* TODO: Test 200Hz */
1609 // inv_set_gyro_sample_rate(5000);
1610 LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate);
1611 LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate);
1612 LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate);
1613
1614 int enabled_sensors = mEnabled;
1615 int tempFd = -1;
1616 if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
1617 if (isLowPowerQuatEnabled() ||
1618 (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) {
1619 bool setDMPrate= 0;
1620 // Set LP Quaternion sample rate if enabled
1621 if (checkLPQuaternion()) {
1622 if (wanted < RATE_200HZ) {
1623 enableLPQuaternion(0);
1624 } else {
1625 inv_set_quat_sample_rate(rateInus);
1626 setDMPrate= 1;
1627 }
1628 }
1629
1630 if (checkDMPOrientation() || setDMPrate==1) {
1631 getDmpRate(&wanted);
1632 }
1633 }
1634
1635 int64_t tempRate = wanted;
1636 LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
1637 //nsToHz
1638 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1639 1000000000.f / tempRate, mpu.gyro_fifo_rate,
1640 getTimestamp());
1641 tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
1642 res = write_attribute_sensor(tempFd, 1000000000.f / tempRate);
1643 if(res < 0) {
1644 LOGE("HAL:GYRO update delay error");
1645 }
1646
1647 //nsToHz (BMA250)
1648 if(USE_THIRD_PARTY_ACCEL) {
1649 LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
1650 wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate,
1651 getTimestamp());
1652 tempFd = open(mpu.accel_fifo_rate, O_RDWR);
1653 res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L);
1654 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
1655 }
1656
1657 if (!mCompassSensor->isIntegrated()) {
1658 LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz", 1000000000.f / wanted_3rd_party_sensor);
1659 mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
1660 got = mCompassSensor->getDelay(ID_M);
1661 inv_set_compass_sample_rate(got / 1000);
1662 }
1663
1664 } else {
1665
1666 if (GY_ENABLED) {
1667 wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
1668 (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
1669 (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
1670
1671 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
1672 getDmpRate(&wanted);
1673 }
1674
1675 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1676 1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp());
1677 tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
1678 res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
1679 LOGE_IF(res < 0, "HAL:GYRO update delay error");
1680 }
1681
1682 if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */
1683 if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
1684 wanted = mDelays[Gyro];
1685 }
1686 else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) {
1687 wanted = mDelays[RawGyro];
1688
1689 } else {
1690 wanted = mDelays[Accelerometer];
1691 }
1692
1693 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
1694 getDmpRate(&wanted);
1695 }
1696
1697 /* TODO: use function pointers to calculate delay value specific to vendor */
1698 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1699 1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp());
1700 tempFd = open(mpu.accel_fifo_rate, O_RDWR);
1701 if(USE_THIRD_PARTY_ACCEL) {
1702 //BMA250 in ms
1703 res = write_attribute_sensor(tempFd, wanted / 1000000L);
1704 }
1705 else {
1706 //MPUxxxx in hz
1707 res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
1708 }
1709 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
1710 }
1711
1712 /* Invensense compass calibration */
1713 if (M_ENABLED) {
1714 if (!mCompassSensor->isIntegrated()) {
1715 wanted = mDelays[MagneticField];
1716 } else {
1717 if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) {
1718 wanted = mDelays[Gyro];
1719 }
1720 else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) {
1721 wanted = mDelays[RawGyro];
1722 } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
1723 wanted = mDelays[Accelerometer];
1724 } else {
1725 wanted = mDelays[MagneticField];
1726 }
1727
1728 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) {
1729 getDmpRate(&wanted);
1730 }
1731 }
1732
1733 mCompassSensor->setDelay(ID_M, wanted);
1734 got = mCompassSensor->getDelay(ID_M);
1735 inv_set_compass_sample_rate(got / 1000);
1736 }
1737
1738 }
1739
1740 unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
1741 if (sensors &
1742 (INV_THREE_AXIS_GYRO
1743 | INV_THREE_AXIS_ACCEL
1744 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
1745 res = masterEnable(1);
1746 } else { // all sensors idle -> reduce power
1747 res = onPower(0);
1748 }
1749 }
1750 pthread_mutex_unlock(&GlobalHalMutex);
1751 return res;
1752 }
1753
1754 /* For Third Party Accel Input Subsystem Drivers only */
1755 /* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */
readAccelEvents(sensors_event_t *,int count)1756 int MPLSensor::readAccelEvents(sensors_event_t* /*data*/, int count)
1757 {
1758 VHANDLER_LOG;
1759
1760 if (count < 1)
1761 return -EINVAL;
1762
1763 ssize_t n = mAccelInputReader.fill(accel_fd);
1764 if (n < 0) {
1765 LOGE("HAL:missed accel events, exit");
1766 return n;
1767 }
1768
1769 int numEventReceived = 0;
1770 input_event const* event;
1771 int nb, done = 0;
1772
1773 while (!done && count && mAccelInputReader.readEvent(&event)) {
1774 int type = event->type;
1775 if (type == EV_ABS) {
1776 if (event->code == EVENT_TYPE_ACCEL_X) {
1777 mPendingMask |= 1 << Accelerometer;
1778 mCachedAccelData[0] = event->value;
1779 } else if (event->code == EVENT_TYPE_ACCEL_Y) {
1780 mPendingMask |= 1 << Accelerometer;
1781 mCachedAccelData[1] = event->value;
1782 } else if (event->code == EVENT_TYPE_ACCEL_Z) {
1783 mPendingMask |= 1 << Accelerometer;
1784 mCachedAccelData[2] =event-> value;
1785 }
1786 } else if (type == EV_SYN) {
1787 done = 1;
1788 if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
1789 inv_build_accel(mCachedAccelData, 0, getTimestamp());
1790 }
1791 } else {
1792 LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)",
1793 type, event->code);
1794 }
1795 mAccelInputReader.next();
1796 }
1797
1798 return numEventReceived;
1799 }
1800
1801 /**
1802 * Should be called after reading at least one of gyro
1803 * compass or accel data. (Also okay for handling all of them).
1804 * @returns 0, if successful, error number if not.
1805 */
1806 /* TODO: This should probably be called "int readEvents(...)"
1807 * and readEvents() called "void cacheData(void)".
1808 */
executeOnData(sensors_event_t * data,int count)1809 int MPLSensor::executeOnData(sensors_event_t* data, int count)
1810 {
1811 VFUNC_LOG;
1812
1813 inv_execute_on_data();
1814
1815 int numEventReceived = 0;
1816
1817 long msg;
1818 msg = inv_get_message_level_0(1);
1819 if (msg) {
1820 if (msg & INV_MSG_MOTION_EVENT) {
1821 LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n");
1822 }
1823 if (msg & INV_MSG_NO_MOTION_EVENT) {
1824 LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n");
1825 /* after the first no motion, the gyro should be
1826 calibrated well */
1827 mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
1828 /* if gyros are on and we got a no motion, set a flag
1829 indicating that the cal file can be written. */
1830 mHaveGoodMpuCal = true;
1831 }
1832 }
1833
1834 // load up virtual sensors
1835 for (int i = 0; i < numSensors; i++) {
1836 int update;
1837 if (mEnabled & (1 << i)) {
1838 update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
1839 mPendingMask |= (1 << i);
1840
1841 if (update && (count > 0)) {
1842 *data++ = mPendingEvents[i];
1843 count--;
1844 numEventReceived++;
1845 }
1846 }
1847 }
1848
1849 return numEventReceived;
1850 }
1851
1852 // collect data for MPL (but NOT sensor service currently), from driver layer
1853 /* TODO: FIX! data and count are not used, results is hardcoded to 0 */
1854 /* TODO: This should probably be called "void cacheEvents(void)"
1855 * And executeOnData() should be int readEvents(data,count)
1856 */
readEvents(sensors_event_t *,int)1857 int MPLSensor::readEvents(sensors_event_t* /*data*/, int /*count*/) {
1858
1859
1860 int lp_quaternion_on = 0, nbyte;
1861 int i, nb, mask = 0, numEventReceived = 0,
1862 sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
1863 ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
1864 (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1 : 0);
1865 char *rdata = mIIOBuffer;
1866
1867 nbyte= (8 * sensors + 8) * 1;
1868
1869 if (isLowPowerQuatEnabled()) {
1870 lp_quaternion_on = checkLPQuaternion();
1871 if (lp_quaternion_on) {
1872 nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data
1873 }
1874 }
1875
1876 // pthread_mutex_lock(&mMplMutex);
1877 // pthread_mutex_lock(&mHALMutex);
1878
1879 ssize_t rsize = read(iio_fd, rdata, nbyte);
1880 if (sensors == 0) {
1881 // read(iio_fd, rdata, nbyte);
1882 rsize = read(iio_fd, rdata, sizeof(mIIOBuffer));
1883 }
1884
1885 #ifdef TESTING
1886 LOGI("get one sample of IIO data with size: %d", rsize);
1887 LOGI("sensors: %d", sensors);
1888
1889 LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d",
1890 *((short *) (rdata + 0)), *((short *) (rdata + 2)),
1891 *((short *) (rdata + 4)));
1892 LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d",
1893 *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
1894 *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
1895 *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
1896
1897 LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS &
1898 mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d",
1899 *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
1900 ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
1901 *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
1902 ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
1903 *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
1904 ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
1905 #endif
1906
1907 if (rsize < (nbyte - 8)) {
1908 LOGE("HAL:ERR Full data packet was not read. rsize=%zd nbyte=%d sensors=%d errno=%d(%s)",
1909 rsize, nbyte, sensors, errno, strerror(errno));
1910 return -1;
1911 }
1912
1913 if (isLowPowerQuatEnabled() && lp_quaternion_on) {
1914
1915 for (i=0; i< 4; i++) {
1916 mCachedQuaternionData[i]= *(long*)rdata;
1917 rdata += sizeof(long);
1918 }
1919 }
1920
1921 for (i = 0; i < 3; i++) {
1922 if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
1923 mCachedGyroData[i] = *((short *) (rdata + i * 2));
1924 }
1925 if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
1926 mCachedAccelData[i] = *((short *) (rdata + i * 2 +
1927 ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
1928 }
1929 if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) {
1930 mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1)));
1931 }
1932 }
1933
1934 mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) +
1935 ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0));
1936 if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
1937 (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
1938 mask |= 1 << MagneticField;
1939 }
1940
1941 mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
1942 if (mCompassSensor->isIntegrated()) {
1943 mCompassTimestamp = mSensorTimestamp;
1944 }
1945
1946 if (mask & (1 << Gyro)) {
1947 // send down temperature every 0.5 seconds
1948 // with timestamp measured in "driver" layer
1949 if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
1950 mTempCurrentTime = mSensorTimestamp;
1951 long long temperature[2];
1952 if(inv_read_temperature(temperature) == 0) {
1953 LOGV_IF(INPUT_DATA,
1954 "HAL:inv_read_temperature = %lld, timestamp= %lld",
1955 temperature[0], temperature[1]);
1956 inv_build_temp(temperature[0], temperature[1]);
1957 }
1958 #ifdef TESTING
1959 long bias[3], temp, temp_slope[3];
1960 inv_get_gyro_bias(bias, &temp);
1961 inv_get_gyro_ts(temp_slope);
1962
1963 LOGI("T: %.3f "
1964 "GB: %+13f %+13f %+13f "
1965 "TS: %+13f %+13f %+13f "
1966 "\n",
1967 (float)temperature[0] / 65536.f,
1968 (float)bias[0] / 65536.f / 16.384f,
1969 (float)bias[1] / 65536.f / 16.384f,
1970 (float)bias[2] / 65536.f / 16.384f,
1971 temp_slope[0] / 65536.f,
1972 temp_slope[1] / 65536.f,
1973 temp_slope[2] / 65536.f);
1974 #endif
1975 }
1976
1977 mPendingMask |= 1 << Gyro;
1978 mPendingMask |= 1 << RawGyro;
1979
1980 if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
1981 inv_build_gyro(mCachedGyroData, mSensorTimestamp);
1982 LOGV_IF(INPUT_DATA,
1983 "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld",
1984 mCachedGyroData[0], mCachedGyroData[1],
1985 mCachedGyroData[2], mSensorTimestamp);
1986 }
1987 }
1988
1989 if (mask & (1 << Accelerometer)) {
1990 mPendingMask |= 1 << Accelerometer;
1991 if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
1992 inv_build_accel(mCachedAccelData, 0, mSensorTimestamp);
1993 LOGV_IF(INPUT_DATA,
1994 "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld",
1995 mCachedAccelData[0], mCachedAccelData[1],
1996 mCachedAccelData[2], mSensorTimestamp);
1997 }
1998 }
1999
2000 if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) {
2001 int status = 0;
2002 if (mCompassSensor->providesCalibration()) {
2003 status = mCompassSensor->getAccuracy();
2004 status |= INV_CALIBRATED;
2005 }
2006 if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
2007 inv_build_compass(mCachedCompassData, status,
2008 mCompassTimestamp);
2009 LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
2010 mCachedCompassData[0], mCachedCompassData[1],
2011 mCachedCompassData[2], mCompassTimestamp);
2012 }
2013 }
2014
2015 if (isLowPowerQuatEnabled() && lp_quaternion_on) {
2016
2017 inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp);
2018 LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld",
2019 mCachedQuaternionData[0], mCachedQuaternionData[1],
2020 mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
2021 }
2022
2023 // pthread_mutex_unlock(&mMplMutex);
2024 // pthread_mutex_unlock(&mHALMutex);
2025
2026 return numEventReceived;
2027 }
2028
2029 /* use for both MPUxxxx and third party compass */
readCompassEvents(sensors_event_t *,int count)2030 int MPLSensor::readCompassEvents(sensors_event_t* /*data*/, int count)
2031 {
2032 VHANDLER_LOG;
2033
2034 if (count < 1)
2035 return -EINVAL;
2036
2037 int numEventReceived = 0;
2038 int done = 0;
2039 int nb;
2040
2041 // pthread_mutex_lock(&mMplMutex);
2042 // pthread_mutex_lock(&mHALMutex);
2043
2044 done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
2045 #ifdef COMPASS_YAS53x
2046 if (mCompassSensor->checkCoilsReset()) {
2047 //Reset relevant compass settings
2048 resetCompass();
2049 }
2050 #endif
2051 if (done > 0) {
2052 int status = 0;
2053 if (mCompassSensor->providesCalibration()) {
2054 status = mCompassSensor->getAccuracy();
2055 status |= INV_CALIBRATED;
2056 }
2057 if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
2058 inv_build_compass(mCachedCompassData, status,
2059 mCompassTimestamp);
2060 LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
2061 mCachedCompassData[0], mCachedCompassData[1],
2062 mCachedCompassData[2], mCompassTimestamp);
2063 }
2064 }
2065
2066 // pthread_mutex_unlock(&mMplMutex);
2067 // pthread_mutex_unlock(&mHALMutex);
2068
2069 return numEventReceived;
2070 }
2071
2072 #ifdef COMPASS_YAS53x
resetCompass()2073 int MPLSensor::resetCompass()
2074 {
2075 VFUNC_LOG;
2076
2077 //Reset compass cal if enabled
2078 if (mFeatureActiveMask & INV_COMPASS_CAL) {
2079 LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
2080 inv_init_vector_compass_cal();
2081 }
2082
2083 //Reset compass fit if enabled
2084 if (mFeatureActiveMask & INV_COMPASS_FIT) {
2085 LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
2086 inv_init_compass_fit();
2087 }
2088
2089 return 0;
2090 }
2091 #endif
2092
getFd() const2093 int MPLSensor::getFd() const
2094 {
2095 VFUNC_LOG;
2096 LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd);
2097 return iio_fd;
2098 }
2099
getAccelFd() const2100 int MPLSensor::getAccelFd() const
2101 {
2102 VFUNC_LOG;
2103 LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd);
2104 return accel_fd;
2105 }
2106
getCompassFd() const2107 int MPLSensor::getCompassFd() const
2108 {
2109 VFUNC_LOG;
2110 int fd = mCompassSensor->getFd();
2111 LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd);
2112 return fd;
2113 }
2114
turnOffAccelFifo()2115 int MPLSensor::turnOffAccelFifo() {
2116 int i, res, tempFd;
2117 char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable,
2118 mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable};
2119
2120 for (i = 0; i < 3; i++) {
2121 res = write_sysfs_int(accel_fifo_enable[i], 0);
2122 if (res < 0) {
2123 return res;
2124 }
2125 }
2126 return 0;
2127 }
2128
enableDmpOrientation(int en)2129 int MPLSensor::enableDmpOrientation(int en)
2130 {
2131 VFUNC_LOG;
2132 /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
2133 int res = 0;
2134 int enabled_sensors = mEnabled;
2135
2136 if (isMpu3050())
2137 return res;
2138
2139 pthread_mutex_lock(&GlobalHalMutex);
2140
2141 // on power if not already On
2142 res = onPower(1);
2143 // reset master enable
2144 res = masterEnable(0);
2145
2146 if (en) {
2147 //Enable DMP orientation
2148 if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
2149 LOGE("HAL:ERR can't enable Android orientation");
2150 res = -1; // indicate an err
2151 }
2152
2153 // open DMP Orient Fd
2154 res = openDmpOrientFd();
2155
2156 // enable DMP
2157 res = onDMP(1);
2158
2159 // default DMP output rate to FIFO
2160 if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) {
2161 LOGE("HAL:ERR can't default DMP output rate");
2162 }
2163
2164 // set DMP rate to 200Hz
2165 if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
2166 res = -1;
2167 LOGE("HAL:ERR can't set DMP rate to 200Hz");
2168 }
2169
2170 // enable accel engine
2171 res = enableAccel(1);
2172
2173 // disable accel FIFO
2174 if (!A_ENABLED) {
2175 res = turnOffAccelFifo();
2176 }
2177
2178 mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
2179 } else {
2180 // disable DMP
2181 res = onDMP(0);
2182
2183 // disable accel engine
2184 if (!A_ENABLED) {
2185 res = enableAccel(0);
2186 }
2187 }
2188
2189 res = masterEnable(1);
2190 pthread_mutex_unlock(&GlobalHalMutex);
2191 return res;
2192 }
2193
openDmpOrientFd()2194 int MPLSensor::openDmpOrientFd()
2195 {
2196 VFUNC_LOG;
2197
2198 if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
2199 LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened");
2200 return -1;
2201 }
2202
2203 dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
2204 if (dmp_orient_fd < 0) {
2205 LOGE("HAL:ERR couldn't open dmpOrient node");
2206 return -1;
2207 } else {
2208 LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
2209 return 0;
2210 }
2211 }
2212
closeDmpOrientFd()2213 int MPLSensor::closeDmpOrientFd()
2214 {
2215 VFUNC_LOG;
2216 if (dmp_orient_fd >= 0)
2217 close(dmp_orient_fd);
2218 return 0;
2219 }
2220
dmpOrientHandler(int orient)2221 int MPLSensor::dmpOrientHandler(int orient)
2222 {
2223 VFUNC_LOG;
2224
2225 LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
2226 return 0;
2227 }
2228
readDmpOrientEvents(sensors_event_t * data,int count)2229 int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) {
2230 VFUNC_LOG;
2231
2232 char dummy[4];
2233 int screen_orientation = 0;
2234 FILE *fp;
2235
2236 fp = fopen(mpu.event_display_orientation, "r");
2237 if (fp == NULL) {
2238 LOGE("HAL:cannot open event_display_orientation");
2239 return 0;
2240 }
2241 fscanf(fp, "%d\n", &screen_orientation);
2242 fclose(fp);
2243
2244 int numEventReceived = 0;
2245
2246 if (mDmpOrientationEnabled && count > 0) {
2247 sensors_event_t temp;
2248
2249 bzero(&temp, sizeof(temp)); /* Let's hope that SENSOR_TYPE_NONE is 0 */
2250 temp.version = sizeof(sensors_event_t);
2251 temp.sensor = ID_SO;
2252 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
2253 temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
2254 temp.screen_orientation = screen_orientation;
2255 #endif
2256 struct timespec ts;
2257 clock_gettime(CLOCK_MONOTONIC, &ts);
2258 temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
2259
2260 *data++ = temp;
2261 count--;
2262 numEventReceived++;
2263 }
2264
2265 // read dummy data per driver's request
2266 dmpOrientHandler(screen_orientation);
2267 read(dmp_orient_fd, dummy, 4);
2268
2269 return numEventReceived;
2270 }
2271
getDmpOrientFd()2272 int MPLSensor::getDmpOrientFd()
2273 {
2274 VFUNC_LOG;
2275
2276 LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd);
2277 return dmp_orient_fd;
2278
2279 }
2280
checkDMPOrientation()2281 int MPLSensor::checkDMPOrientation()
2282 {
2283 VFUNC_LOG;
2284 return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
2285 }
2286
getDmpRate(int64_t * wanted)2287 int MPLSensor::getDmpRate(int64_t *wanted)
2288 {
2289 if (checkDMPOrientation() || checkLPQuaternion()) {
2290 // set DMP output rate to FIFO
2291 write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted);
2292 LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *wanted);
2293
2294 //DMP running rate must be @ 200Hz
2295 *wanted= RATE_200HZ;
2296 LOGV_IF(PROCESS_VERBOSE, "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
2297 }
2298 return 0;
2299 }
2300
getPollTime()2301 int MPLSensor::getPollTime()
2302 {
2303 VHANDLER_LOG;
2304 return mPollTime;
2305 }
2306
hasPendingEvents() const2307 bool MPLSensor::hasPendingEvents() const
2308 {
2309 VHANDLER_LOG;
2310 // if we are using the polling workaround, force the main
2311 // loop to check for data every time
2312 return (mPollTime != -1);
2313 }
2314
2315 /* TODO: support resume suspend when we gain more info about them*/
sleepEvent()2316 void MPLSensor::sleepEvent()
2317 {
2318 VFUNC_LOG;
2319 }
2320
wakeEvent()2321 void MPLSensor::wakeEvent()
2322 {
2323 VFUNC_LOG;
2324 }
2325
inv_float_to_q16(float * fdata,long * ldata)2326 int MPLSensor::inv_float_to_q16(float *fdata, long *ldata)
2327 {
2328 VHANDLER_LOG;
2329
2330 if (!fdata || !ldata)
2331 return -1;
2332 ldata[0] = (long)(fdata[0] * 65536.f);
2333 ldata[1] = (long)(fdata[1] * 65536.f);
2334 ldata[2] = (long)(fdata[2] * 65536.f);
2335 return 0;
2336 }
2337
inv_long_to_q16(long * fdata,long * ldata)2338 int MPLSensor::inv_long_to_q16(long *fdata, long *ldata)
2339 {
2340 VHANDLER_LOG;
2341
2342 if (!fdata || !ldata)
2343 return -1;
2344 ldata[0] = (fdata[1] * 65536.f);
2345 ldata[1] = (fdata[2] * 65536.f);
2346 ldata[2] = (fdata[3] * 65536.f);
2347 return 0;
2348 }
2349
inv_float_to_round(float * fdata,long * ldata)2350 int MPLSensor::inv_float_to_round(float *fdata, long *ldata)
2351 {
2352 VHANDLER_LOG;
2353
2354 if (!fdata || !ldata)
2355 return -1;
2356 ldata[0] = (long)fdata[0];
2357 ldata[1] = (long)fdata[1];
2358 ldata[2] = (long)fdata[2];
2359 return 0;
2360 }
2361
inv_float_to_round2(float * fdata,short * ldata)2362 int MPLSensor::inv_float_to_round2(float *fdata, short *ldata)
2363 {
2364 VHANDLER_LOG;
2365
2366 if (!fdata || !ldata)
2367 return -1;
2368 ldata[0] = (short)fdata[0];
2369 ldata[1] = (short)fdata[1];
2370 ldata[2] = (short)fdata[2];
2371 return 0;
2372 }
2373
inv_read_temperature(long long * data)2374 int MPLSensor::inv_read_temperature(long long *data)
2375 {
2376 VHANDLER_LOG;
2377
2378 int count = 0;
2379 char raw_buf[40];
2380 long raw = 0;
2381
2382 long long timestamp = 0;
2383
2384 memset(raw_buf, 0, sizeof(raw_buf));
2385 count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
2386 sizeof(raw_buf));
2387 if(count < 1) {
2388 LOGE("HAL:error reading gyro temperature");
2389 return -1;
2390 }
2391
2392 count = sscanf(raw_buf, "%ld%lld", &raw, ×tamp);
2393
2394 if(count < 0) {
2395 return -1;
2396 }
2397
2398 LOGV_IF(ENG_VERBOSE,
2399 "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
2400 raw, timestamp, count);
2401 data[0] = raw;
2402 data[1] = timestamp;
2403
2404 return 0;
2405 }
2406
inv_read_dmp_state(int fd)2407 int MPLSensor::inv_read_dmp_state(int fd)
2408 {
2409 VFUNC_LOG;
2410
2411 if(fd < 0)
2412 return -1;
2413
2414 int count = 0;
2415 char raw_buf[10];
2416 short raw = 0;
2417
2418 memset(raw_buf, 0, sizeof(raw_buf));
2419 count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
2420 if(count < 1) {
2421 LOGE("HAL:error reading dmp state");
2422 close(fd);
2423 return -1;
2424 }
2425 count = sscanf(raw_buf, "%hd", &raw);
2426 if(count < 0) {
2427 LOGE("HAL:dmp state data is invalid");
2428 close(fd);
2429 return -1;
2430 }
2431 LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
2432 close(fd);
2433 return (int)raw;
2434 }
2435
inv_read_sensor_bias(int fd,long * data)2436 int MPLSensor::inv_read_sensor_bias(int fd, long *data)
2437 {
2438 VFUNC_LOG;
2439
2440 if(fd == -1) {
2441 return -1;
2442 }
2443
2444 char buf[50];
2445 char x[15], y[15], z[15];
2446
2447 memset(buf, 0, sizeof(buf));
2448 int count = read_attribute_sensor(fd, buf, sizeof(buf));
2449 if(count < 1) {
2450 LOGE("HAL:Error reading gyro bias");
2451 return -1;
2452 }
2453 count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
2454 if(count) {
2455 /* scale appropriately for MPL */
2456 LOGV_IF(ENG_VERBOSE,
2457 "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
2458 atol(x), atol(y), atol(z));
2459
2460 data[0] = (long)(atol(x) / 10000 * (1L << 16));
2461 data[1] = (long)(atol(y) / 10000 * (1L << 16));
2462 data[2] = (long)(atol(z) / 10000 * (1L << 16));
2463
2464 LOGV_IF(ENG_VERBOSE,
2465 "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
2466 data[0], data[1], data[2]);
2467 }
2468 return 0;
2469 }
2470
2471 /** fill in the sensor list based on which sensors are configured.
2472 * return the number of configured sensors.
2473 * parameter list must point to a memory region of at least 7*sizeof(sensor_t)
2474 * parameter len gives the length of the buffer pointed to by list
2475 */
populateSensorList(struct sensor_t * list,int len)2476 int MPLSensor::populateSensorList(struct sensor_t *list, int len)
2477 {
2478 VFUNC_LOG;
2479
2480 int numsensors;
2481
2482 if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
2483 LOGE("HAL:sensor list too small, not populating.");
2484 return -(sizeof(sSensorList) / sizeof(sensor_t));
2485 }
2486
2487 /* fill in the base values */
2488 memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t)));
2489
2490 /* first add gyro, accel and compass to the list */
2491
2492 /* fill in gyro/accel values */
2493 if(chip_ID == NULL) {
2494 LOGE("HAL:Can not get gyro/accel id");
2495 }
2496 fillGyro(chip_ID, list);
2497 fillAccel(chip_ID, list);
2498
2499 // TODO: need fixes for unified HAL and 3rd-party solution
2500 mCompassSensor->fillList(&list[MagneticField]);
2501
2502 if(1) {
2503 numsensors = (sizeof(sSensorList) / sizeof(sensor_t));
2504 /* all sensors will be added to the list
2505 fill in orientation values */
2506 fillOrientation(list);
2507 /* fill in rotation vector values */
2508 fillRV(list);
2509 /* fill in gravity values */
2510 fillGravity(list);
2511 /* fill in Linear accel values */
2512 fillLinearAccel(list);
2513 } else {
2514 /* no 9-axis sensors, zero fill that part of the list */
2515 numsensors = 3;
2516 memset(list + 3, 0, 4 * sizeof(struct sensor_t));
2517 }
2518
2519 return numsensors;
2520 }
2521
fillAccel(const char * accel,struct sensor_t * list)2522 void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
2523 {
2524 VFUNC_LOG;
2525
2526 if (accel) {
2527 if(accel != NULL && strcmp(accel, "BMA250") == 0) {
2528 list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
2529 list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
2530 list[Accelerometer].power = ACCEL_BMA250_POWER;
2531 list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
2532 return;
2533 } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
2534 list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
2535 list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
2536 list[Accelerometer].power = ACCEL_MPU6050_POWER;
2537 list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
2538 return;
2539 } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
2540 list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
2541 list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
2542 list[Accelerometer].power = ACCEL_MPU6500_POWER;
2543 list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
2544
2545 return;
2546 } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
2547 list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
2548 list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
2549 list[Accelerometer].power = ACCEL_MPU9150_POWER;
2550 list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
2551 return;
2552 } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
2553 list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
2554 list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
2555 list[Accelerometer].power = ACCEL_BMA250_POWER;
2556 list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
2557 return;
2558 }
2559 }
2560
2561 LOGE("HAL:unknown accel id %s -- "
2562 "params default to bma250 and might be wrong.",
2563 accel);
2564 list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
2565 list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
2566 list[Accelerometer].power = ACCEL_BMA250_POWER;
2567 list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
2568 }
2569
fillGyro(const char * gyro,struct sensor_t * list)2570 void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
2571 {
2572 VFUNC_LOG;
2573
2574 if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
2575 list[Gyro].maxRange = GYRO_MPU3050_RANGE;
2576 list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
2577 list[Gyro].power = GYRO_MPU3050_POWER;
2578 list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
2579 } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
2580 list[Gyro].maxRange = GYRO_MPU6050_RANGE;
2581 list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
2582 list[Gyro].power = GYRO_MPU6050_POWER;
2583 list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
2584 } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
2585 list[Gyro].maxRange = GYRO_MPU6500_RANGE;
2586 list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
2587 list[Gyro].power = GYRO_MPU6500_POWER;
2588 list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
2589 } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
2590 list[Gyro].maxRange = GYRO_MPU9150_RANGE;
2591 list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
2592 list[Gyro].power = GYRO_MPU9150_POWER;
2593 list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
2594 } else {
2595 LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
2596 LOGE("HAL:default to use mpu3050 params");
2597 list[Gyro].maxRange = GYRO_MPU3050_RANGE;
2598 list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
2599 list[Gyro].power = GYRO_MPU3050_POWER;
2600 list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
2601 }
2602
2603 list[RawGyro].maxRange = list[Gyro].maxRange;
2604 list[RawGyro].resolution = list[Gyro].resolution;
2605 list[RawGyro].power = list[Gyro].power;
2606 list[RawGyro].minDelay = list[Gyro].minDelay;
2607
2608 return;
2609 }
2610
2611 /* fillRV depends on values of accel and compass in the list */
fillRV(struct sensor_t * list)2612 void MPLSensor::fillRV(struct sensor_t *list)
2613 {
2614 VFUNC_LOG;
2615
2616 /* compute power on the fly */
2617 list[RotationVector].power = list[Gyro].power +
2618 list[Accelerometer].power +
2619 list[MagneticField].power;
2620 list[RotationVector].resolution = .00001;
2621 list[RotationVector].maxRange = 1.0;
2622 list[RotationVector].minDelay = 5000;
2623
2624 return;
2625 }
2626
fillOrientation(struct sensor_t * list)2627 void MPLSensor::fillOrientation(struct sensor_t *list)
2628 {
2629 VFUNC_LOG;
2630
2631 list[Orientation].power = list[Gyro].power +
2632 list[Accelerometer].power +
2633 list[MagneticField].power;
2634 list[Orientation].resolution = .00001;
2635 list[Orientation].maxRange = 360.0;
2636 list[Orientation].minDelay = 5000;
2637
2638 return;
2639 }
2640
fillGravity(struct sensor_t * list)2641 void MPLSensor::fillGravity( struct sensor_t *list)
2642 {
2643 VFUNC_LOG;
2644
2645 list[Gravity].power = list[Gyro].power +
2646 list[Accelerometer].power +
2647 list[MagneticField].power;
2648 list[Gravity].resolution = .00001;
2649 list[Gravity].maxRange = 9.81;
2650 list[Gravity].minDelay = 5000;
2651
2652 return;
2653 }
2654
fillLinearAccel(struct sensor_t * list)2655 void MPLSensor::fillLinearAccel(struct sensor_t *list)
2656 {
2657 VFUNC_LOG;
2658
2659 list[LinearAccel].power = list[Gyro].power +
2660 list[Accelerometer].power +
2661 list[MagneticField].power;
2662 list[LinearAccel].resolution = list[Accelerometer].resolution;
2663 list[LinearAccel].maxRange = list[Accelerometer].maxRange;
2664 list[LinearAccel].minDelay = 5000;
2665
2666 return;
2667 }
2668
inv_init_sysfs_attributes(void)2669 int MPLSensor::inv_init_sysfs_attributes(void)
2670 {
2671 VFUNC_LOG;
2672
2673 unsigned char i;
2674 char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN];
2675 char *sptr;
2676 char **dptr;
2677 int num;
2678
2679 sysfs_names_ptr =
2680 (char*)calloc(1, sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
2681 sptr = sysfs_names_ptr;
2682 if (sptr == NULL) {
2683 LOGE("HAL:couldn't alloc mem for sysfs paths");
2684 return -1;
2685 }
2686
2687 dptr = (char**)&mpu;
2688 i = 0;
2689 do {
2690 *dptr++ = sptr;
2691 sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
2692 } while (++i < MAX_SYSFS_ATTRB);
2693
2694 // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
2695 // inv_get_sysfs_abs_path(sysfs_path);
2696 if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
2697 ALOGE("MPLSensor failed get sysfs path");
2698 return -1;
2699 }
2700
2701 if(INV_SUCCESS != inv_get_iio_trigger_path(iio_trigger_path)) {
2702 ALOGE("MPLSensor failed get iio trigger path");
2703 return -1;
2704 }
2705
2706 sprintf(mpu.key, "%s%s", sysfs_path, "/key");
2707 sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
2708 sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
2709 sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
2710 sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
2711 sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name");
2712 sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger");
2713
2714 sprintf(mpu.dmp_firmware, "%s%s", sysfs_path,"/dmp_firmware");
2715 sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded");
2716 sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
2717 sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on");
2718 sprintf(mpu.dmp_event_int_on,"%s%s", sysfs_path, "/dmp_event_int_on");
2719 sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate");
2720 sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
2721
2722 // TODO: for self test
2723 sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
2724
2725 sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
2726 sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
2727 sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
2728 sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
2729 sprintf(mpu.gyro_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en");
2730 sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en");
2731 sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en");
2732
2733 sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
2734 sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
2735 sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
2736
2737
2738 #ifndef THIRD_PARTY_ACCEL //MPUxxxx
2739 sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
2740 // TODO: for bias settings
2741 sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias");
2742 #endif
2743
2744 sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
2745 sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
2746 sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");
2747
2748 sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on");
2749 sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en");
2750 sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en");
2751 sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en");
2752 sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en");
2753
2754 sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
2755 sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
2756
2757 #if SYSFS_VERBOSE
2758 // test print sysfs paths
2759 dptr = (char**)&mpu;
2760 for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
2761 LOGE("HAL:sysfs path: %s", *dptr++);
2762 }
2763 #endif
2764 return 0;
2765 }
2766
2767 /* TODO: stop manually testing/using 0 and 1 instead of
2768 * false and true, but just use 0 and non-0.
2769 * This allows passing 0 and non-0 ints around instead of
2770 * having to convert to 1 and test against 1.
2771 */
isMpu3050()2772 bool MPLSensor::isMpu3050()
2773 {
2774 return !strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050");
2775 }
2776
isLowPowerQuatEnabled()2777 int MPLSensor::isLowPowerQuatEnabled()
2778 {
2779 #ifdef ENABLE_LP_QUAT_FEAT
2780 return !isMpu3050();
2781 #else
2782 return 0;
2783 #endif
2784 }
2785
isDmpDisplayOrientationOn()2786 int MPLSensor::isDmpDisplayOrientationOn()
2787 {
2788 #ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
2789 return !isMpu3050();
2790 #else
2791 return 0;
2792 #endif
2793 }
2794