1 /*
2 * Copyright (C) 2014 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
19 //see also the EXTRA_VERBOSE define in the MPLSensor.h header file
20
21 #include <fcntl.h>
22 #include <errno.h>
23 #include <math.h>
24 #include <float.h>
25 #include <poll.h>
26 #include <unistd.h>
27 #include <dirent.h>
28 #include <stdlib.h>
29 #include <sys/select.h>
30 #include <sys/syscall.h>
31 #include <dlfcn.h>
32 #include <pthread.h>
33 #include <cutils/log.h>
34 #include <utils/KeyedVector.h>
35 #include <utils/Vector.h>
36 #include <utils/String8.h>
37 #include <string.h>
38 #include <linux/input.h>
39 #include <utils/Atomic.h>
40 #include <utils/SystemClock.h>
41
42 #include "MPLSensor.h"
43 #include "PressureSensor.IIO.secondary.h"
44 #include "MPLSupport.h"
45 #include "sensor_params.h"
46
47 #include "invensense.h"
48 #include "invensense_adv.h"
49 #include "ml_stored_data.h"
50 #include "ml_load_dmp.h"
51 #include "ml_sysfs_helper.h"
52
53 #define ENABLE_MULTI_RATE
54 // #define TESTING
55 #define USE_LPQ_AT_FASTEST
56
57 #ifdef THIRD_PARTY_ACCEL
58 #pragma message("HAL:build third party accel support")
59 #define USE_THIRD_PARTY_ACCEL (1)
60 #else
61 #define USE_THIRD_PARTY_ACCEL (0)
62 #endif
63
64 #define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))
65
66 // query path to determine if vibrator is currently vibrating
67 #define VIBRATOR_ENABLE_FILE "/sys/class/timed_output/vibrator/enable"
68
69
70 // Minimum time after vibrator triggers SMD before SMD can be declared valid
71 // This allows 100mS for events to propogate
72 #define MIN_TRIGGER_TIME_AFTER_VIBRATOR_NS 100000000
73
74
75 /******************************************************************************/
76 /* MPL Interface */
77 /******************************************************************************/
78
79 //#define INV_PLAYBACK_DBG
80 #ifdef INV_PLAYBACK_DBG
81 static FILE *logfile = NULL;
82 #endif
83
84 /*******************************************************************************
85 * MPLSensor class implementation
86 ******************************************************************************/
87
88 static int64_t mt_pre_ns;
89
90 // following extended initializer list would only be available with -std=c++11
91 // or -std=gnu+11
MPLSensor(CompassSensor * compass,int (* m_pt2AccelCalLoadFunc)(long *))92 MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *))
93 : SensorBase(NULL, NULL),
94 mMasterSensorMask(INV_ALL_SENSORS),
95 mLocalSensorMask(0),
96 mPollTime(-1),
97 mStepCountPollTime(-1),
98 mHaveGoodMpuCal(0),
99 mGyroAccuracy(0),
100 mAccelAccuracy(0),
101 mCompassAccuracy(0),
102 dmp_orient_fd(-1),
103 mDmpOrientationEnabled(0),
104 dmp_sign_motion_fd(-1),
105 mDmpSignificantMotionEnabled(0),
106 dmp_pedometer_fd(-1),
107 mDmpPedometerEnabled(0),
108 mDmpStepCountEnabled(0),
109 mEnabled(0),
110 mEnabledCached(0),
111 mBatchEnabled(0),
112 mOldBatchEnabledMask(0),
113 mAccelInputReader(4),
114 mGyroInputReader(32),
115 mTempScale(0),
116 mTempOffset(0),
117 mTempCurrentTime(0),
118 mAccelScale(2),
119 mAccelSelfTestScale(2),
120 mGyroScale(2000),
121 mGyroSelfTestScale(2000),
122 mCompassScale(0),
123 mFactoryGyroBiasAvailable(false),
124 mGyroBiasAvailable(false),
125 mGyroBiasApplied(false),
126 mFactoryAccelBiasAvailable(false),
127 mAccelBiasAvailable(false),
128 mAccelBiasApplied(false),
129 mPendingMask(0),
130 mSensorMask(0),
131 mGyroBatchRate(0),
132 mAccelBatchRate(0),
133 mCompassBatchRate(0),
134 mPressureBatchRate(0),
135 mQuatBatchRate(0),
136 mGyroRate(0),
137 mAccelRate(0),
138 mCompassRate(0),
139 mPressureRate(0),
140 mQuatRate(0),
141 mResetRate(0),
142 mDataInterrupt(0),
143 mFirstBatchCall(1),
144 mEnableCalled(1),
145 mMplFeatureActiveMask(0),
146 mFeatureActiveMask(0),
147 mDmpOn(0),
148 mPedUpdate(0),
149 mPressureUpdate(0),
150 mQuatSensorTimestamp(0),
151 mStepSensorTimestamp(0),
152 mLastStepCount(-1),
153 mLeftOverBufferSize(0),
154 mInitial6QuatValueAvailable(0),
155 mSkipReadEvents(0),
156 mSkipExecuteOnData(0),
157 mDataMarkerDetected(0),
158 mEmptyDataMarkerDetected(0) {
159 VFUNC_LOG;
160
161 inv_error_t rv;
162 int i, fd;
163 char *port = NULL;
164 char *ver_str;
165 unsigned long mSensorMask;
166 int res;
167 FILE *fptr;
168
169 mCompassSensor = compass;
170
171 LOGV_IF(EXTRA_VERBOSE,
172 "HAL:MPLSensor constructor : NumSensors = %d", NumSensors);
173
174 pthread_mutex_init(&mMplMutex, NULL);
175 pthread_mutex_init(&mHALMutex, NULL);
176 mFlushBatchSet = 0;
177 memset(mGyroOrientation, 0, sizeof(mGyroOrientation));
178 memset(mAccelOrientation, 0, sizeof(mAccelOrientation));
179 memset(mInitial6QuatValue, 0, sizeof(mInitial6QuatValue));
180 mFlushSensorEnabledVector.setCapacity(NumSensors);
181 memset(mEnabledTime, 0, sizeof(mEnabledTime));
182 memset(mLastTimestamp, 0, sizeof(mLastTimestamp));
183
184 /* setup sysfs paths */
185 inv_init_sysfs_attributes();
186
187 /* get chip name */
188 if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
189 LOGE("HAL:ERR- Failed to get chip ID\n");
190 } else {
191 LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID);
192 }
193
194 enable_iio_sysfs();
195
196 #ifdef ENABLE_PRESSURE
197 /* instantiate pressure sensor on secondary bus */
198 mPressureSensor = new PressureSensor((const char*)mSysfsPath);
199 #endif
200
201 /* reset driver master enable */
202 masterEnable(0);
203
204 /* Load DMP image if capable, ie. MPU6515 */
205 loadDMP();
206
207 /* open temperature fd for temp comp */
208 LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
209 gyro_temperature_fd = open(mpu.temperature, O_RDONLY);
210 if (gyro_temperature_fd == -1) {
211 LOGE("HAL:could not open temperature node");
212 } else {
213 LOGV_IF(EXTRA_VERBOSE,
214 "HAL:temperature_fd opened: %s", mpu.temperature);
215 }
216
217 /* read gyro FSR to calculate accel scale later */
218 char gyroBuf[5];
219 int count = 0;
220 LOGV_IF(SYSFS_VERBOSE,
221 "HAL:sysfs:cat %s (%lld)", mpu.gyro_fsr, getTimestamp());
222
223 fd = open(mpu.gyro_fsr, O_RDONLY);
224 if(fd < 0) {
225 LOGE("HAL:Error opening gyro FSR");
226 } else {
227 memset(gyroBuf, 0, sizeof(gyroBuf));
228 count = read_attribute_sensor(fd, gyroBuf, sizeof(gyroBuf));
229 if(count < 1) {
230 LOGE("HAL:Error reading gyro FSR");
231 } else {
232 count = sscanf(gyroBuf, "%ld", &mGyroScale);
233 if(count)
234 LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro FSR used %ld", mGyroScale);
235 }
236 close(fd);
237 }
238
239 /* read gyro self test scale used to calculate factory cal bias later */
240 char gyroScale[5];
241 LOGV_IF(SYSFS_VERBOSE,
242 "HAL:sysfs:cat %s (%lld)", mpu.in_gyro_self_test_scale, getTimestamp());
243 fd = open(mpu.in_gyro_self_test_scale, O_RDONLY);
244 if(fd < 0) {
245 LOGE("HAL:Error opening gyro self test scale");
246 } else {
247 memset(gyroScale, 0, sizeof(gyroScale));
248 count = read_attribute_sensor(fd, gyroScale, sizeof(gyroScale));
249 if(count < 1) {
250 LOGE("HAL:Error reading gyro self test scale");
251 } else {
252 count = sscanf(gyroScale, "%ld", &mGyroSelfTestScale);
253 if(count)
254 LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro self test scale used %ld", mGyroSelfTestScale);
255 }
256 close(fd);
257 }
258
259 /* open Factory Gyro Bias fd */
260 /* mFactoryGyBias contains bias values that will be used for device offset */
261 memset(mFactoryGyroBias, 0, sizeof(mFactoryGyroBias));
262 LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro x offset path: %s", mpu.in_gyro_x_offset);
263 LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro y offset path: %s", mpu.in_gyro_y_offset);
264 LOGV_IF(EXTRA_VERBOSE, "HAL:factory gyro z offset path: %s", mpu.in_gyro_z_offset);
265 gyro_x_offset_fd = open(mpu.in_gyro_x_offset, O_RDWR);
266 gyro_y_offset_fd = open(mpu.in_gyro_y_offset, O_RDWR);
267 gyro_z_offset_fd = open(mpu.in_gyro_z_offset, O_RDWR);
268 if (gyro_x_offset_fd == -1 ||
269 gyro_y_offset_fd == -1 || gyro_z_offset_fd == -1) {
270 LOGE("HAL:could not open factory gyro calibrated bias");
271 } else {
272 LOGV_IF(EXTRA_VERBOSE,
273 "HAL:gyro_offset opened");
274 }
275
276 /* open Gyro Bias fd */
277 /* mGyroBias contains bias values that will be used for framework */
278 /* mGyroChipBias contains bias values that will be used for dmp */
279 memset(mGyroBias, 0, sizeof(mGyroBias));
280 memset(mGyroChipBias, 0, sizeof(mGyroChipBias));
281 LOGV_IF(EXTRA_VERBOSE, "HAL: gyro x dmp bias path: %s", mpu.in_gyro_x_dmp_bias);
282 LOGV_IF(EXTRA_VERBOSE, "HAL: gyro y dmp bias path: %s", mpu.in_gyro_y_dmp_bias);
283 LOGV_IF(EXTRA_VERBOSE, "HAL: gyro z dmp bias path: %s", mpu.in_gyro_z_dmp_bias);
284 gyro_x_dmp_bias_fd = open(mpu.in_gyro_x_dmp_bias, O_RDWR);
285 gyro_y_dmp_bias_fd = open(mpu.in_gyro_y_dmp_bias, O_RDWR);
286 gyro_z_dmp_bias_fd = open(mpu.in_gyro_z_dmp_bias, O_RDWR);
287 if (gyro_x_dmp_bias_fd == -1 ||
288 gyro_y_dmp_bias_fd == -1 || gyro_z_dmp_bias_fd == -1) {
289 LOGE("HAL:could not open gyro DMP calibrated bias");
290 } else {
291 LOGV_IF(EXTRA_VERBOSE,
292 "HAL:gyro_dmp_bias opened");
293 }
294
295 /* read accel FSR to calcuate accel scale later */
296 if (USE_THIRD_PARTY_ACCEL == 0) {
297 char buf[3];
298 int count = 0;
299 LOGV_IF(SYSFS_VERBOSE,
300 "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());
301
302 fd = open(mpu.accel_fsr, O_RDONLY);
303 if(fd < 0) {
304 LOGE("HAL:Error opening accel FSR");
305 } else {
306 memset(buf, 0, sizeof(buf));
307 count = read_attribute_sensor(fd, buf, sizeof(buf));
308 if(count < 1) {
309 LOGE("HAL:Error reading accel FSR");
310 } else {
311 count = sscanf(buf, "%d", &mAccelScale);
312 if(count)
313 LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
314 }
315 close(fd);
316 }
317
318 /* read accel self test scale used to calculate factory cal bias later */
319 char accelScale[5];
320 LOGV_IF(SYSFS_VERBOSE,
321 "HAL:sysfs:cat %s (%lld)", mpu.in_accel_self_test_scale, getTimestamp());
322 fd = open(mpu.in_accel_self_test_scale, O_RDONLY);
323 if(fd < 0) {
324 LOGE("HAL:Error opening gyro self test scale");
325 } else {
326 memset(accelScale, 0, sizeof(accelScale));
327 count = read_attribute_sensor(fd, accelScale, sizeof(accelScale));
328 if(count < 1) {
329 LOGE("HAL:Error reading accel self test scale");
330 } else {
331 count = sscanf(accelScale, "%ld", &mAccelSelfTestScale);
332 if(count)
333 LOGV_IF(EXTRA_VERBOSE, "HAL:Accel self test scale used %ld", mAccelSelfTestScale);
334 }
335 close(fd);
336 }
337
338 /* open Factory Accel Bias fd */
339 /* mFactoryAccelBias contains bias values that will be used for device offset */
340 memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias));
341 LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel x offset path: %s", mpu.in_accel_x_offset);
342 LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel y offset path: %s", mpu.in_accel_y_offset);
343 LOGV_IF(EXTRA_VERBOSE, "HAL:factory accel z offset path: %s", mpu.in_accel_z_offset);
344 accel_x_offset_fd = open(mpu.in_accel_x_offset, O_RDWR);
345 accel_y_offset_fd = open(mpu.in_accel_y_offset, O_RDWR);
346 accel_z_offset_fd = open(mpu.in_accel_z_offset, O_RDWR);
347 if (accel_x_offset_fd == -1 ||
348 accel_y_offset_fd == -1 || accel_z_offset_fd == -1) {
349 LOGE("HAL:could not open factory accel calibrated bias");
350 } else {
351 LOGV_IF(EXTRA_VERBOSE,
352 "HAL:accel_offset opened");
353 }
354
355 /* open Accel Bias fd */
356 /* mAccelBias contains bias that will be used for dmp */
357 memset(mAccelBias, 0, sizeof(mAccelBias));
358 LOGV_IF(EXTRA_VERBOSE, "HAL:accel x dmp bias path: %s", mpu.in_accel_x_dmp_bias);
359 LOGV_IF(EXTRA_VERBOSE, "HAL:accel y dmp bias path: %s", mpu.in_accel_y_dmp_bias);
360 LOGV_IF(EXTRA_VERBOSE, "HAL:accel z dmp bias path: %s", mpu.in_accel_z_dmp_bias);
361 accel_x_dmp_bias_fd = open(mpu.in_accel_x_dmp_bias, O_RDWR);
362 accel_y_dmp_bias_fd = open(mpu.in_accel_y_dmp_bias, O_RDWR);
363 accel_z_dmp_bias_fd = open(mpu.in_accel_z_dmp_bias, O_RDWR);
364 if (accel_x_dmp_bias_fd == -1 ||
365 accel_y_dmp_bias_fd == -1 || accel_z_dmp_bias_fd == -1) {
366 LOGE("HAL:could not open accel DMP calibrated bias");
367 } else {
368 LOGV_IF(EXTRA_VERBOSE,
369 "HAL:accel_dmp_bias opened");
370 }
371 }
372
373 dmp_sign_motion_fd = open(mpu.event_smd, O_RDONLY | O_NONBLOCK);
374 if (dmp_sign_motion_fd < 0) {
375 LOGE("HAL:ERR couldn't open dmp_sign_motion node");
376 } else {
377 LOGV_IF(ENG_VERBOSE,
378 "HAL:dmp_sign_motion_fd opened : %d", dmp_sign_motion_fd);
379 }
380
381 /* the following threshold can be modified for SMD sensitivity */
382 int motionThreshold = 3000;
383 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
384 motionThreshold, mpu.smd_threshold, getTimestamp());
385 res = write_sysfs_int(mpu.smd_threshold, motionThreshold);
386
387 #if 0
388 int StepCounterThreshold = 5;
389 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
390 StepCounterThreshold, mpu.pedometer_step_thresh, getTimestamp());
391 res = write_sysfs_int(mpu.pedometer_step_thresh, StepCounterThreshold);
392 #endif
393
394 dmp_pedometer_fd = open(mpu.event_pedometer, O_RDONLY | O_NONBLOCK);
395 if (dmp_pedometer_fd < 0) {
396 LOGE("HAL:ERR couldn't open dmp_pedometer node");
397 } else {
398 LOGV_IF(ENG_VERBOSE,
399 "HAL:dmp_pedometer_fd opened : %d", dmp_pedometer_fd);
400 }
401
402 initBias();
403
404 (void)inv_get_version(&ver_str);
405 LOGI("%s\n", ver_str);
406
407 /* setup MPL */
408 inv_constructor_init();
409
410 #ifdef INV_PLAYBACK_DBG
411 LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
412 logfile = fopen("/data/playback.bin", "w+");
413 if (logfile)
414 inv_turn_on_data_logging(logfile);
415 #endif
416
417 /* setup orientation matrix and scale */
418 inv_set_device_properties();
419
420 /* initialize sensor data */
421 memset(mPendingEvents, 0, sizeof(mPendingEvents));
422 memset(mPendingFlushEvents, 0, sizeof(mPendingFlushEvents));
423
424 mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
425 mPendingEvents[RotationVector].sensor = ID_RV;
426 mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
427 mPendingEvents[RotationVector].acceleration.status
428 = SENSOR_STATUS_ACCURACY_HIGH;
429
430 mPendingEvents[GameRotationVector].version = sizeof(sensors_event_t);
431 mPendingEvents[GameRotationVector].sensor = ID_GRV;
432 mPendingEvents[GameRotationVector].type = SENSOR_TYPE_GAME_ROTATION_VECTOR;
433 mPendingEvents[GameRotationVector].acceleration.status
434 = SENSOR_STATUS_ACCURACY_HIGH;
435
436 mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
437 mPendingEvents[LinearAccel].sensor = ID_LA;
438 mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
439 mPendingEvents[LinearAccel].acceleration.status
440 = SENSOR_STATUS_ACCURACY_HIGH;
441
442 mPendingEvents[Gravity].version = sizeof(sensors_event_t);
443 mPendingEvents[Gravity].sensor = ID_GR;
444 mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
445 mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
446
447 mPendingEvents[Gyro].version = sizeof(sensors_event_t);
448 mPendingEvents[Gyro].sensor = ID_GY;
449 mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
450 mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
451
452 mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
453 mPendingEvents[RawGyro].sensor = ID_RG;
454 mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
455 mPendingEvents[RawGyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
456
457 mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
458 mPendingEvents[Accelerometer].sensor = ID_A;
459 mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
460 mPendingEvents[Accelerometer].acceleration.status
461 = SENSOR_STATUS_ACCURACY_HIGH;
462
463 /* Invensense compass calibration */
464 mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
465 mPendingEvents[MagneticField].sensor = ID_M;
466 mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
467 mPendingEvents[MagneticField].magnetic.status =
468 SENSOR_STATUS_ACCURACY_HIGH;
469
470 mPendingEvents[RawMagneticField].version = sizeof(sensors_event_t);
471 mPendingEvents[RawMagneticField].sensor = ID_RM;
472 mPendingEvents[RawMagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED;
473 mPendingEvents[RawMagneticField].magnetic.status =
474 SENSOR_STATUS_ACCURACY_HIGH;
475
476 #ifdef ENABLE_PRESSURE
477 mPendingEvents[Pressure].version = sizeof(sensors_event_t);
478 mPendingEvents[Pressure].sensor = ID_PS;
479 mPendingEvents[Pressure].type = SENSOR_TYPE_PRESSURE;
480 mPendingEvents[Pressure].magnetic.status =
481 SENSOR_STATUS_ACCURACY_HIGH;
482 #endif
483
484 mPendingEvents[Orientation].version = sizeof(sensors_event_t);
485 mPendingEvents[Orientation].sensor = ID_O;
486 mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
487 mPendingEvents[Orientation].orientation.status
488 = SENSOR_STATUS_ACCURACY_HIGH;
489
490 mPendingEvents[GeomagneticRotationVector].version = sizeof(sensors_event_t);
491 mPendingEvents[GeomagneticRotationVector].sensor = ID_GMRV;
492 mPendingEvents[GeomagneticRotationVector].type
493 = SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
494 mPendingEvents[GeomagneticRotationVector].acceleration.status
495 = SENSOR_STATUS_ACCURACY_HIGH;
496
497 mSmEvents.version = sizeof(sensors_event_t);
498 mSmEvents.sensor = ID_SM;
499 mSmEvents.type = SENSOR_TYPE_SIGNIFICANT_MOTION;
500 mSmEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE;
501
502 mSdEvents.version = sizeof(sensors_event_t);
503 mSdEvents.sensor = ID_P;
504 mSdEvents.type = SENSOR_TYPE_STEP_DETECTOR;
505 mSdEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE;
506
507 mScEvents.version = sizeof(sensors_event_t);
508 mScEvents.sensor = ID_SC;
509 mScEvents.type = SENSOR_TYPE_STEP_COUNTER;
510 mScEvents.acceleration.status = SENSOR_STATUS_UNRELIABLE;
511
512 /* Event Handlers for HW and Virtual Sensors */
513 mHandlers[RotationVector] = &MPLSensor::rvHandler;
514 mHandlers[GameRotationVector] = &MPLSensor::grvHandler;
515 mHandlers[LinearAccel] = &MPLSensor::laHandler;
516 mHandlers[Gravity] = &MPLSensor::gravHandler;
517 mHandlers[Gyro] = &MPLSensor::gyroHandler;
518 mHandlers[RawGyro] = &MPLSensor::rawGyroHandler;
519 mHandlers[Accelerometer] = &MPLSensor::accelHandler;
520 mHandlers[MagneticField] = &MPLSensor::compassHandler;
521 mHandlers[RawMagneticField] = &MPLSensor::rawCompassHandler;
522 mHandlers[Orientation] = &MPLSensor::orienHandler;
523 mHandlers[GeomagneticRotationVector] = &MPLSensor::gmHandler;
524 #ifdef ENABLE_PRESSURE
525 mHandlers[Pressure] = &MPLSensor::psHandler;
526 #endif
527
528 /* initialize delays to reasonable values */
529 for (int i = 0; i < NumSensors; i++) {
530 mDelays[i] = 1000000000LL;
531 mBatchDelays[i] = 1000000000LL;
532 mBatchTimeouts[i] = 100000000000LL;
533 }
534
535 /* initialize Compass Bias */
536 memset(mCompassBias, 0, sizeof(mCompassBias));
537
538 /* initialize Factory Accel Bias */
539 memset(mFactoryAccelBias, 0, sizeof(mFactoryAccelBias));
540
541 /* initialize Gyro Bias */
542 memset(mGyroBias, 0, sizeof(mGyroBias));
543 memset(mGyroChipBias, 0, sizeof(mGyroChipBias));
544
545 /* load calibration file from /data/inv_cal_data.bin */
546 rv = inv_load_calibration();
547 if(rv == INV_SUCCESS) {
548 LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
549 /* Get initial values */
550 getCompassBias();
551 getGyroBias();
552 if (mGyroBiasAvailable) {
553 setGyroBias();
554 }
555 getAccelBias();
556 getFactoryGyroBias();
557 if (mFactoryGyroBiasAvailable) {
558 setFactoryGyroBias();
559 }
560 getFactoryAccelBias();
561 if (mFactoryAccelBiasAvailable) {
562 setFactoryAccelBias();
563 }
564 }
565 else
566 LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);
567
568 /* takes external accel calibration load workflow */
569 if( m_pt2AccelCalLoadFunc != NULL) {
570 long accel_offset[3];
571 long tmp_offset[3];
572 int result = m_pt2AccelCalLoadFunc(accel_offset);
573 if(result)
574 LOGW("HAL:Vendor accelerometer calibration file load failed %d\n",
575 result);
576 else {
577 LOGW("HAL:Vendor accelerometer calibration file successfully "
578 "loaded");
579 inv_get_mpl_accel_bias(tmp_offset, NULL);
580 LOGV_IF(PROCESS_VERBOSE,
581 "HAL:Original accel offset, %ld, %ld, %ld\n",
582 tmp_offset[0], tmp_offset[1], tmp_offset[2]);
583 inv_set_accel_bias_mask(accel_offset, mAccelAccuracy,4);
584 inv_get_mpl_accel_bias(tmp_offset, NULL);
585 LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n",
586 tmp_offset[0], tmp_offset[1], tmp_offset[2]);
587 }
588 }
589 /* end of external accel calibration load workflow */
590
591 /* disable all sensors and features */
592 masterEnable(0);
593 enableGyro(0);
594 enableLowPowerAccel(0);
595 enableAccel(0);
596 enableCompass(0,0);
597 #ifdef ENABLE_PRESSURE
598 enablePressure(0);
599 #endif
600 enableBatch(0);
601
602 if (isLowPowerQuatEnabled()) {
603 enableLPQuaternion(0);
604 }
605
606 if (isDmpDisplayOrientationOn()) {
607 // open DMP Orient Fd
608 openDmpOrientFd();
609 enableDmpOrientation(!isDmpScreenAutoRotationEnabled());
610 }
611 }
612
enable_iio_sysfs(void)613 void MPLSensor::enable_iio_sysfs(void)
614 {
615 VFUNC_LOG;
616
617 char iio_device_node[MAX_CHIP_ID_LEN];
618 FILE *tempFp = NULL;
619
620 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
621 mpu.in_timestamp_en, getTimestamp());
622 // Either fopen()/open() are okay for sysfs access
623 // developers could choose what they want
624 // with fopen(), the benefit is that fprintf()/fscanf() are available
625 tempFp = fopen(mpu.in_timestamp_en, "w");
626 if (tempFp == NULL) {
627 LOGE("HAL:could not open timestamp enable");
628 } else {
629 if(fprintf(tempFp, "%d", 1) < 0) {
630 LOGE("HAL:could not enable timestamp");
631 }
632 if(fclose(tempFp) < 0) {
633 LOGE("HAL:could not close timestamp");
634 }
635 }
636
637 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
638 IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp());
639 tempFp = fopen(mpu.buffer_length, "w");
640 if (tempFp == NULL) {
641 LOGE("HAL:could not open buffer length");
642 } else {
643 if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) {
644 LOGE("HAL:could not write buffer length");
645 }
646 if (fclose(tempFp) < 0) {
647 LOGE("HAL:could not close buffer length");
648 }
649 }
650
651 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
652 1, mpu.chip_enable, getTimestamp());
653 tempFp = fopen(mpu.chip_enable, "w");
654 if (tempFp == NULL) {
655 LOGE("HAL:could not open chip enable");
656 } else {
657 if (fprintf(tempFp, "%d", 1) < 0) {
658 LOGE("HAL:could not write chip enable");
659 }
660 if (fclose(tempFp) < 0) {
661 LOGE("HAL:could not close chip enable");
662 }
663 }
664
665 inv_get_iio_device_node(iio_device_node);
666 iio_fd = open(iio_device_node, O_RDONLY);
667 if (iio_fd < 0) {
668 LOGE("HAL:could not open iio device node");
669 } else {
670 LOGV_IF(ENG_VERBOSE, "HAL:iio iio_fd opened : %d", iio_fd);
671 }
672 }
673
inv_constructor_init(void)674 int MPLSensor::inv_constructor_init(void)
675 {
676 VFUNC_LOG;
677
678 inv_error_t result = inv_init_mpl();
679 if (result) {
680 LOGE("HAL:inv_init_mpl() failed");
681 return result;
682 }
683 result = inv_constructor_default_enable();
684 result = inv_start_mpl();
685 if (result) {
686 LOGE("HAL:inv_start_mpl() failed");
687 LOG_RESULT_LOCATION(result);
688 return result;
689 }
690
691 return result;
692 }
693
inv_constructor_default_enable(void)694 int MPLSensor::inv_constructor_default_enable(void)
695 {
696 VFUNC_LOG;
697
698 inv_error_t result;
699
700 /*******************************************************************************
701
702 ********************************************************************************
703
704 The InvenSense binary file (libmplmpu.so) is subject to Google's standard terms
705 and conditions as accepted in the click-through agreement required to download
706 this library.
707 The library includes, but is not limited to the following function calls:
708 inv_enable_quaternion().
709
710 ANY VIOLATION OF SUCH TERMS AND CONDITIONS WILL BE STRICTLY ENFORCED.
711
712 ********************************************************************************
713
714 *******************************************************************************/
715
716 result = inv_enable_quaternion();
717 if (result) {
718 LOGE("HAL:Cannot enable quaternion\n");
719 return result;
720 }
721
722 result = inv_enable_in_use_auto_calibration();
723 if (result) {
724 return result;
725 }
726
727 result = inv_enable_fast_nomot();
728 if (result) {
729 return result;
730 }
731
732 result = inv_enable_gyro_tc();
733 if (result) {
734 return result;
735 }
736
737 result = inv_enable_hal_outputs();
738 if (result) {
739 return result;
740 }
741
742 if (!mCompassSensor->providesCalibration()) {
743 /* Invensense compass calibration */
744 LOGV_IF(ENG_VERBOSE, "HAL:Invensense vector compass cal enabled");
745 result = inv_enable_vector_compass_cal();
746 if (result) {
747 LOG_RESULT_LOCATION(result);
748 return result;
749 } else {
750 mMplFeatureActiveMask |= INV_COMPASS_CAL;
751 }
752 // specify MPL's trust weight, used by compass algorithms
753 inv_vector_compass_cal_sensitivity(3);
754
755 /* disabled by default
756 result = inv_enable_compass_bias_w_gyro();
757 if (result) {
758 LOG_RESULT_LOCATION(result);
759 return result;
760 }
761 */
762
763 result = inv_enable_heading_from_gyro();
764 if (result) {
765 LOG_RESULT_LOCATION(result);
766 return result;
767 }
768
769 result = inv_enable_magnetic_disturbance();
770 if (result) {
771 LOG_RESULT_LOCATION(result);
772 return result;
773 }
774 //inv_enable_magnetic_disturbance_logging();
775 }
776
777 result = inv_enable_9x_sensor_fusion();
778 if (result) {
779 LOG_RESULT_LOCATION(result);
780 return result;
781 } else {
782 // 9x sensor fusion enables Compass fit
783 mMplFeatureActiveMask |= INV_COMPASS_FIT;
784 }
785
786 result = inv_enable_no_gyro_fusion();
787 if (result) {
788 LOG_RESULT_LOCATION(result);
789 return result;
790 }
791
792 return result;
793 }
794
795 /* TODO: create function pointers to calculate scale */
inv_set_device_properties(void)796 void MPLSensor::inv_set_device_properties(void)
797 {
798 VFUNC_LOG;
799
800 unsigned short orient;
801
802 inv_get_sensors_orientation();
803
804 inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE);
805 inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE);
806
807 /* gyro setup */
808 orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
809 inv_set_gyro_orientation_and_scale(orient, mGyroScale << 15);
810 LOGI_IF(EXTRA_VERBOSE, "HAL: Set MPL Gyro Scale %ld", mGyroScale << 15);
811
812 /* accel setup */
813 orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
814 /* use for third party accel input subsystem driver
815 inv_set_accel_orientation_and_scale(orient, 1LL << 22);
816 */
817 inv_set_accel_orientation_and_scale(orient, (long)mAccelScale << 15);
818 LOGI_IF(EXTRA_VERBOSE,
819 "HAL: Set MPL Accel Scale %ld", (long)mAccelScale << 15);
820
821 /* compass setup */
822 signed char orientMtx[9];
823 mCompassSensor->getOrientationMatrix(orientMtx);
824 orient =
825 inv_orientation_matrix_to_scalar(orientMtx);
826 long sensitivity;
827 sensitivity = mCompassSensor->getSensitivity();
828 inv_set_compass_orientation_and_scale(orient, sensitivity);
829 mCompassScale = sensitivity;
830 LOGI_IF(EXTRA_VERBOSE,
831 "HAL: Set MPL Compass Scale %ld", mCompassScale);
832 }
833
loadDMP(void)834 void MPLSensor::loadDMP(void)
835 {
836 VFUNC_LOG;
837
838 int res, fd;
839 FILE *fptr;
840
841 if (isMpuNonDmp()) {
842 return;
843 }
844
845 /* load DMP firmware */
846 LOGV_IF(SYSFS_VERBOSE,
847 "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
848 fd = open(mpu.firmware_loaded, O_RDONLY);
849 if(fd < 0) {
850 LOGE("HAL:could not open dmp state");
851 } else {
852 if(inv_read_dmp_state(fd) == 0) {
853 LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
854 fptr = fopen(mpu.dmp_firmware, "w");
855 if(fptr == NULL) {
856 LOGE("HAL:could not open dmp_firmware");
857 } else {
858 if (inv_load_dmp(fptr) < 0) {
859 LOGE("HAL:load DMP failed");
860 } else {
861 LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
862 }
863 if (fclose(fptr) < 0) {
864 LOGE("HAL:could not close dmp firmware");
865 }
866 }
867 } else {
868 LOGV_IF(ENG_VERBOSE, "HAL:DMP is already loaded");
869 }
870 }
871
872 // onDmp(1); //Can't enable here. See note onDmp()
873 }
874
inv_get_sensors_orientation(void)875 void MPLSensor::inv_get_sensors_orientation(void)
876 {
877 VFUNC_LOG;
878
879 FILE *fptr;
880
881 // get gyro orientation
882 LOGV_IF(SYSFS_VERBOSE,
883 "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
884 fptr = fopen(mpu.gyro_orient, "r");
885 if (fptr != NULL) {
886 int om[9];
887 if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
888 &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
889 &om[6], &om[7], &om[8]) < 0) {
890 LOGE("HAL:Could not read gyro mounting matrix");
891 } else {
892 LOGV_IF(EXTRA_VERBOSE,
893 "HAL:gyro mounting matrix: "
894 "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
895 om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
896
897 mGyroOrientation[0] = om[0];
898 mGyroOrientation[1] = om[1];
899 mGyroOrientation[2] = om[2];
900 mGyroOrientation[3] = om[3];
901 mGyroOrientation[4] = om[4];
902 mGyroOrientation[5] = om[5];
903 mGyroOrientation[6] = om[6];
904 mGyroOrientation[7] = om[7];
905 mGyroOrientation[8] = om[8];
906 }
907 if (fclose(fptr) < 0) {
908 LOGE("HAL:Could not close gyro mounting matrix");
909 }
910 }
911
912 // get accel orientation
913 LOGV_IF(SYSFS_VERBOSE,
914 "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
915 fptr = fopen(mpu.accel_orient, "r");
916 if (fptr != NULL) {
917 int om[9];
918 if (fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
919 &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
920 &om[6], &om[7], &om[8]) < 0) {
921 LOGE("HAL:could not read accel mounting matrix");
922 } else {
923 LOGV_IF(EXTRA_VERBOSE,
924 "HAL:accel mounting matrix: "
925 "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
926 om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);
927
928 mAccelOrientation[0] = om[0];
929 mAccelOrientation[1] = om[1];
930 mAccelOrientation[2] = om[2];
931 mAccelOrientation[3] = om[3];
932 mAccelOrientation[4] = om[4];
933 mAccelOrientation[5] = om[5];
934 mAccelOrientation[6] = om[6];
935 mAccelOrientation[7] = om[7];
936 mAccelOrientation[8] = om[8];
937 }
938 if (fclose(fptr) < 0) {
939 LOGE("HAL:could not close accel mounting matrix");
940 }
941 }
942 }
943
~MPLSensor()944 MPLSensor::~MPLSensor()
945 {
946 VFUNC_LOG;
947
948 /* Close open fds */
949 if (iio_fd > 0)
950 close(iio_fd);
951 if( accel_fd > 0 )
952 close(accel_fd );
953 if (gyro_temperature_fd > 0)
954 close(gyro_temperature_fd);
955 if (sysfs_names_ptr)
956 free(sysfs_names_ptr);
957
958 closeDmpOrientFd();
959
960 if (accel_x_dmp_bias_fd > 0) {
961 close(accel_x_dmp_bias_fd);
962 }
963 if (accel_y_dmp_bias_fd > 0) {
964 close(accel_y_dmp_bias_fd);
965 }
966 if (accel_z_dmp_bias_fd > 0) {
967 close(accel_z_dmp_bias_fd);
968 }
969
970 if (gyro_x_dmp_bias_fd > 0) {
971 close(gyro_x_dmp_bias_fd);
972 }
973 if (gyro_y_dmp_bias_fd > 0) {
974 close(gyro_y_dmp_bias_fd);
975 }
976 if (gyro_z_dmp_bias_fd > 0) {
977 close(gyro_z_dmp_bias_fd);
978 }
979
980 if (gyro_x_offset_fd > 0) {
981 close(gyro_x_offset_fd);
982 }
983 if (gyro_y_offset_fd > 0) {
984 close(gyro_y_offset_fd);
985 }
986 if (gyro_z_offset_fd > 0) {
987 close(gyro_z_offset_fd);
988 }
989
990 /* Turn off Gyro master enable */
991 /* A workaround until driver handles it */
992 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
993 0, mpu.master_enable, getTimestamp());
994 write_sysfs_int(mpu.master_enable, 0);
995
996 #ifdef INV_PLAYBACK_DBG
997 inv_turn_off_data_logging();
998 if (fclose(logfile) < 0) {
999 LOGE("cannot close debug log file");
1000 }
1001 #endif
1002 }
1003
1004 #define GY_ENABLED ((1 << ID_GY) & enabled_sensors)
1005 #define RGY_ENABLED ((1 << ID_RG) & enabled_sensors)
1006 #define A_ENABLED ((1 << ID_A) & enabled_sensors)
1007 #define M_ENABLED ((1 << ID_M) & enabled_sensors)
1008 #define RM_ENABLED ((1 << ID_RM) & enabled_sensors)
1009 #define O_ENABLED ((1 << ID_O) & enabled_sensors)
1010 #define LA_ENABLED ((1 << ID_LA) & enabled_sensors)
1011 #define GR_ENABLED ((1 << ID_GR) & enabled_sensors)
1012 #define RV_ENABLED ((1 << ID_RV) & enabled_sensors)
1013 #define GRV_ENABLED ((1 << ID_GRV) & enabled_sensors)
1014 #define GMRV_ENABLED ((1 << ID_GMRV) & enabled_sensors)
1015
1016 #ifdef ENABLE_PRESSURE
1017 #define PS_ENABLED ((1 << ID_PS) & enabled_sensors)
1018 #endif
1019
1020 /* this applies to BMA250 Input Subsystem Driver only */
setAccelInitialState()1021 int MPLSensor::setAccelInitialState()
1022 {
1023 VFUNC_LOG;
1024
1025 struct input_absinfo absinfo_x;
1026 struct input_absinfo absinfo_y;
1027 struct input_absinfo absinfo_z;
1028 float value;
1029 if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) &&
1030 !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) &&
1031 !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) {
1032 value = absinfo_x.value;
1033 mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X;
1034 value = absinfo_y.value;
1035 mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y;
1036 value = absinfo_z.value;
1037 mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z;
1038 //mHasPendingEvent = true;
1039 }
1040 return 0;
1041 }
1042
onDmp(int en)1043 int MPLSensor::onDmp(int en)
1044 {
1045 VFUNC_LOG;
1046
1047 int res = -1;
1048 int status;
1049 mDmpOn = en;
1050
1051 //Sequence to enable DMP
1052 //1. Load DMP image if not already loaded
1053 //2. Either Gyro or Accel must be enabled/configured before next step
1054 //3. Enable DMP
1055
1056 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
1057 mpu.firmware_loaded, getTimestamp());
1058 if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){
1059 LOGE("HAL:ERR can't get firmware_loaded status");
1060 } else if (status == 1) {
1061 //Write only if curr DMP state <> request
1062 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
1063 mpu.dmp_on, getTimestamp());
1064 if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
1065 LOGE("HAL:ERR can't read DMP state");
1066 } else if (status != en) {
1067 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1068 en, mpu.dmp_on, getTimestamp());
1069 if (write_sysfs_int(mpu.dmp_on, en) < 0) {
1070 LOGE("HAL:ERR can't write dmp_on");
1071 } else {
1072 mDmpOn = en;
1073 res = 0; //Indicate write successful
1074 if(!en) {
1075 setAccelBias();
1076 }
1077 }
1078 //Enable DMP interrupt
1079 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1080 en, mpu.dmp_int_on, getTimestamp());
1081 if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
1082 LOGE("HAL:ERR can't en/dis DMP interrupt");
1083 }
1084
1085 // disable DMP event interrupt if needed
1086 if (!en) {
1087 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1088 en, mpu.dmp_event_int_on, getTimestamp());
1089 if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
1090 res = -1;
1091 LOGE("HAL:ERR can't enable DMP event interrupt");
1092 }
1093 }
1094 } else {
1095 mDmpOn = en;
1096 res = 0; //DMP already set as requested
1097 if(!en) {
1098 setAccelBias();
1099 }
1100 }
1101 } else {
1102 LOGE("HAL:ERR No DMP image");
1103 }
1104 return res;
1105 }
1106
setDmpFeature(int en)1107 int MPLSensor::setDmpFeature(int en)
1108 {
1109 int res = 0;
1110
1111 // set sensor engine and fifo
1112 if (((mFeatureActiveMask & ~INV_DMP_BATCH_MODE) & DMP_FEATURE_MASK) || en) {
1113 if ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) ||
1114 (mFeatureActiveMask & INV_DMP_PED_QUATERNION) ||
1115 (mFeatureActiveMask & INV_DMP_QUATERNION)) {
1116 res = enableGyro(1);
1117 if (res < 0) {
1118 return res;
1119 }
1120 if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) {
1121 res = turnOffGyroFifo();
1122 if (res < 0) {
1123 return res;
1124 }
1125 }
1126 }
1127 res = enableAccel(1);
1128 if (res < 0) {
1129 return res;
1130 }
1131 if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
1132 res = turnOffAccelFifo();
1133 if (res < 0) {
1134 return res;
1135 }
1136 }
1137 } else {
1138 if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) {
1139 res = enableGyro(0);
1140 if (res < 0) {
1141 return res;
1142 }
1143 }
1144 if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
1145 res = enableAccel(0);
1146 if (res < 0) {
1147 return res;
1148 }
1149 }
1150 }
1151
1152 // set sensor data interrupt
1153 uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE));
1154 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1155 !dataInterrupt, mpu.dmp_event_int_on, getTimestamp());
1156 if (write_sysfs_int(mpu.dmp_event_int_on, !dataInterrupt) < 0) {
1157 res = -1;
1158 LOGE("HAL:ERR can't enable DMP event interrupt");
1159 }
1160 return res;
1161 }
1162
computeDmpState(bool * dmp_state)1163 int MPLSensor::computeDmpState(bool* dmp_state)
1164 {
1165 int res = 0;
1166 bool dmpState = 0;
1167
1168 if (mFeatureActiveMask) {
1169 dmpState = 1;
1170 LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() mFeatureActiveMask = 1");
1171 } else if ((mEnabled & VIRTUAL_SENSOR_9AXES_MASK)
1172 || (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK)) {
1173 if (checkLPQuaternion() && checkLPQRateSupported()) {
1174 dmpState = 1;
1175 LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() Sensor Fusion = 1");
1176 }
1177 } /*else {
1178 unsigned long sensor = mLocalSensorMask & mMasterSensorMask;
1179 if (sensor & (INV_THREE_AXIS_ACCEL & INV_THREE_AXIS_GYRO)) {
1180 dmpState = 1;
1181 LOGV_IF(PROCESS_VERBOSE, "HAL:computeAndSetDmpState() accel and gyro = 1");
1182 }
1183 }*/
1184
1185 *dmp_state = dmpState;
1186
1187 return res;
1188 }
1189
SetDmpState(bool dmpState)1190 int MPLSensor::SetDmpState(bool dmpState)
1191 {
1192 int res = 0;
1193
1194 // set Dmp state
1195 res = onDmp(dmpState);
1196 if (res < 0)
1197 return res;
1198
1199 if (dmpState) {
1200 // set DMP rate to 200Hz
1201 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1202 200, mpu.accel_fifo_rate, getTimestamp());
1203 if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
1204 res = -1;
1205 LOGE("HAL:ERR can't set rate to 200Hz");
1206 return res;
1207 }
1208 }
1209 LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is set %s", (dmpState ? "on" : "off"));
1210 mDmpState = dmpState;
1211 return dmpState;
1212
1213 }
1214
computeAndSetDmpState()1215 int MPLSensor::computeAndSetDmpState()
1216 {
1217 int res = 0;
1218 bool dmpState = 0;
1219
1220 computeDmpState(&dmpState);
1221
1222 res = SetDmpState(dmpState);
1223 if (res < 0)
1224 return res;
1225
1226 return dmpState;
1227 }
1228
1229 /* called when batch and hw sensor enabled*/
enablePedIndicator(int en)1230 int MPLSensor::enablePedIndicator(int en)
1231 {
1232 VFUNC_LOG;
1233
1234 int res = 0;
1235 if (en) {
1236 if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) {
1237 //Disable DMP Pedometer Interrupt
1238 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1239 0, mpu.pedometer_int_on, getTimestamp());
1240 if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
1241 LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
1242 res = -1; // indicate an err
1243 return res;
1244 }
1245
1246 LOGV_IF(ENG_VERBOSE, "HAL:Enabling ped standalone");
1247 // enable accel engine
1248 res = enableAccel(1);
1249 if (res < 0)
1250 return res;
1251 LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask);
1252 // disable accel FIFO
1253 if (!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) {
1254 res = turnOffAccelFifo();
1255 if (res < 0)
1256 return res;
1257 }
1258 }
1259 } else {
1260 //Disable Accel if no sensor needs it
1261 if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1262 && (!(mLocalSensorMask & mMasterSensorMask
1263 & INV_THREE_AXIS_ACCEL))) {
1264 res = enableAccel(0);
1265 if (res < 0)
1266 return res;
1267 }
1268 }
1269
1270 LOGV_IF(ENG_VERBOSE, "HAL:Toggling step indicator to %d", en);
1271 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1272 en, mpu.step_indicator_on, getTimestamp());
1273 if (write_sysfs_int(mpu.step_indicator_on, en) < 0) {
1274 res = -1;
1275 LOGE("HAL:ERR can't write to DMP step_indicator_on");
1276 }
1277 return res;
1278 }
1279
checkPedStandaloneBatched(void)1280 int MPLSensor::checkPedStandaloneBatched(void)
1281 {
1282 VFUNC_LOG;
1283 int res = 0;
1284
1285 if ((mFeatureActiveMask & INV_DMP_PEDOMETER) &&
1286 (mBatchEnabled & (1 << StepDetector))) {
1287 res = 1;
1288 } else
1289 res = 0;
1290
1291 LOGV_IF(ENG_VERBOSE, "HAL:checkPedStandaloneBatched=%d", res);
1292 return res;
1293 }
1294
checkPedStandaloneEnabled(void)1295 int MPLSensor::checkPedStandaloneEnabled(void)
1296 {
1297 VFUNC_LOG;
1298 return ((mFeatureActiveMask & INV_DMP_PED_STANDALONE)? 1:0);
1299 }
1300
1301 /* This feature is only used in batch mode */
1302 /* Stand-alone Step Detector */
enablePedStandalone(int en)1303 int MPLSensor::enablePedStandalone(int en)
1304 {
1305 VFUNC_LOG;
1306
1307 if (!en) {
1308 enablePedStandaloneData(0);
1309 mFeatureActiveMask &= ~INV_DMP_PED_STANDALONE;
1310 if (mFeatureActiveMask == 0) {
1311 onDmp(0);
1312 } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) {
1313 //Re-enable DMP Pedometer Interrupt
1314 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1315 1, mpu.pedometer_int_on, getTimestamp());
1316 if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) {
1317 LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
1318 return (-1);
1319 }
1320 //Disable data interrupt if no continuous data
1321 if (mEnabled == 0) {
1322 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1323 1, mpu.dmp_event_int_on, getTimestamp());
1324 if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
1325 LOGE("HAL:ERR can't enable DMP event interrupt");
1326 return (-1);
1327 }
1328 }
1329 }
1330 LOGV_IF(ENG_VERBOSE, "HAL:Ped Standalone disabled");
1331 } else {
1332 if (enablePedStandaloneData(1) < 0 || onDmp(1) < 0) {
1333 LOGE("HAL:ERR can't enable Ped Standalone");
1334 } else {
1335 mFeatureActiveMask |= INV_DMP_PED_STANDALONE;
1336 //Disable DMP Pedometer Interrupt
1337 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1338 0, mpu.pedometer_int_on, getTimestamp());
1339 if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
1340 LOGE("HAL:ERR can't disable Android Pedometer Interrupt");
1341 return (-1);
1342 }
1343 //Enable Data Interrupt
1344 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1345 0, mpu.dmp_event_int_on, getTimestamp());
1346 if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
1347 LOGE("HAL:ERR can't enable DMP event interrupt");
1348 return (-1);
1349 }
1350 LOGV_IF(ENG_VERBOSE, "HAL:Ped Standalone enabled");
1351 }
1352 }
1353 return 0;
1354 }
1355
enablePedStandaloneData(int en)1356 int MPLSensor:: enablePedStandaloneData(int en)
1357 {
1358 VFUNC_LOG;
1359
1360 int res = 0;
1361
1362 // Set DMP Ped standalone
1363 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1364 en, mpu.step_detector_on, getTimestamp());
1365 if (write_sysfs_int(mpu.step_detector_on, en) < 0) {
1366 LOGE("HAL:ERR can't write DMP step_detector_on");
1367 res = -1; //Indicate an err
1368 }
1369
1370 // Set DMP Step indicator
1371 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1372 en, mpu.step_indicator_on, getTimestamp());
1373 if (write_sysfs_int(mpu.step_indicator_on, en) < 0) {
1374 LOGE("HAL:ERR can't write DMP step_indicator_on");
1375 res = -1; //Indicate an err
1376 }
1377
1378 if (!en) {
1379 LOGV_IF(ENG_VERBOSE, "HAL:Disabling ped standalone");
1380 //Disable Accel if no sensor needs it
1381 if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1382 && (!(mLocalSensorMask & mMasterSensorMask
1383 & INV_THREE_AXIS_ACCEL))) {
1384 res = enableAccel(0);
1385 if (res < 0)
1386 return res;
1387 }
1388 if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1389 && (!(mLocalSensorMask & mMasterSensorMask
1390 & INV_THREE_AXIS_GYRO))) {
1391 res = enableGyro(0);
1392 if (res < 0)
1393 return res;
1394 }
1395 } else {
1396 LOGV_IF(ENG_VERBOSE, "HAL:Enabling ped standalone");
1397 // enable accel engine
1398 res = enableAccel(1);
1399 if (res < 0)
1400 return res;
1401 LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask);
1402 // disable accel FIFO
1403 if (!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) {
1404 res = turnOffAccelFifo();
1405 if (res < 0)
1406 return res;
1407 }
1408 }
1409
1410 return res;
1411 }
1412
checkPedQuatEnabled(void)1413 int MPLSensor::checkPedQuatEnabled(void)
1414 {
1415 VFUNC_LOG;
1416 return ((mFeatureActiveMask & INV_DMP_PED_QUATERNION)? 1:0);
1417 }
1418
1419 /* This feature is only used in batch mode */
1420 /* Step Detector && Game Rotation Vector */
enablePedQuaternion(int en)1421 int MPLSensor::enablePedQuaternion(int en)
1422 {
1423 VFUNC_LOG;
1424
1425 if (!en) {
1426 enablePedQuaternionData(0);
1427 mFeatureActiveMask &= ~INV_DMP_PED_QUATERNION;
1428 if (mFeatureActiveMask == 0) {
1429 onDmp(0);
1430 } else if(mFeatureActiveMask & INV_DMP_PEDOMETER) {
1431 //Re-enable DMP Pedometer Interrupt
1432 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1433 1, mpu.pedometer_int_on, getTimestamp());
1434 if (write_sysfs_int(mpu.pedometer_int_on, 1) < 0) {
1435 LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
1436 return (-1);
1437 }
1438 //Disable data interrupt if no continuous data
1439 if (mEnabled == 0) {
1440 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1441 1, mpu.dmp_event_int_on, getTimestamp());
1442 if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
1443 LOGE("HAL:ERR can't enable DMP event interrupt");
1444 return (-1);
1445 }
1446 }
1447 }
1448 LOGV_IF(ENG_VERBOSE, "HAL:Ped Quat disabled");
1449 } else {
1450 if (enablePedQuaternionData(1) < 0 || onDmp(1) < 0) {
1451 LOGE("HAL:ERR can't enable Ped Quaternion");
1452 } else {
1453 mFeatureActiveMask |= INV_DMP_PED_QUATERNION;
1454 //Disable DMP Pedometer Interrupt
1455 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1456 0, mpu.pedometer_int_on, getTimestamp());
1457 if (write_sysfs_int(mpu.pedometer_int_on, 0) < 0) {
1458 LOGE("HAL:ERR can't disable Android Pedometer Interrupt");
1459 return (-1);
1460 }
1461 //Enable Data Interrupt
1462 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1463 0, mpu.dmp_event_int_on, getTimestamp());
1464 if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
1465 LOGE("HAL:ERR can't enable DMP event interrupt");
1466 return (-1);
1467 }
1468 LOGV_IF(ENG_VERBOSE, "HAL:Ped Quat enabled");
1469 }
1470 }
1471 return 0;
1472 }
1473
enablePedQuaternionData(int en)1474 int MPLSensor::enablePedQuaternionData(int en)
1475 {
1476 VFUNC_LOG;
1477
1478 int res = 0;
1479
1480 // Enable DMP quaternion
1481 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1482 en, mpu.ped_q_on, getTimestamp());
1483 if (write_sysfs_int(mpu.ped_q_on, en) < 0) {
1484 LOGE("HAL:ERR can't write DMP ped_q_on");
1485 res = -1; //Indicate an err
1486 }
1487
1488 if (!en) {
1489 LOGV_IF(ENG_VERBOSE, "HAL:Disabling ped quat");
1490 //Disable Accel if no sensor needs it
1491 if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1492 && (!(mLocalSensorMask & mMasterSensorMask
1493 & INV_THREE_AXIS_ACCEL))) {
1494 res = enableAccel(0);
1495 if (res < 0)
1496 return res;
1497 }
1498 if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1499 && (!(mLocalSensorMask & mMasterSensorMask
1500 & INV_THREE_AXIS_GYRO))) {
1501 res = enableGyro(0);
1502 if (res < 0)
1503 return res;
1504 }
1505 if (mFeatureActiveMask & INV_DMP_QUATERNION) {
1506 res = write_sysfs_int(mpu.gyro_fifo_enable, 1);
1507 res += write_sysfs_int(mpu.accel_fifo_enable, 1);
1508 if (res < 0)
1509 return res;
1510 }
1511 // LOGV_IF(ENG_VERBOSE, "before mLocalSensorMask=0x%lx", mLocalSensorMask);
1512 // reset global mask for buildMpuEvent()
1513 if (mEnabled & (1 << GameRotationVector)) {
1514 mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1515 mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1516 } else if (mEnabled & (1 << Accelerometer)) {
1517 mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1518 } else if ((mEnabled & ( 1 << Gyro)) ||
1519 (mEnabled & (1 << RawGyro))) {
1520 mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1521 }
1522 //LOGV_IF(ENG_VERBOSE, "after mLocalSensorMask=0x%lx", mLocalSensorMask);
1523 } else {
1524 LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling ped quat");
1525 // enable accel engine
1526 res = enableAccel(1);
1527 if (res < 0)
1528 return res;
1529
1530 // enable gyro engine
1531 res = enableGyro(1);
1532 if (res < 0)
1533 return res;
1534 LOGV_IF(EXTRA_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask);
1535 // disable accel FIFO
1536 if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_ACCEL)) ||
1537 !(mBatchEnabled & (1 << Accelerometer))) {
1538 res = turnOffAccelFifo();
1539 if (res < 0)
1540 return res;
1541 mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
1542 }
1543
1544 // disable gyro FIFO
1545 if ((!((mLocalSensorMask & mMasterSensorMask) & INV_THREE_AXIS_GYRO)) ||
1546 !((mBatchEnabled & (1 << Gyro)) || (mBatchEnabled & (1 << RawGyro)))) {
1547 res = turnOffGyroFifo();
1548 if (res < 0)
1549 return res;
1550 mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
1551 }
1552 }
1553
1554 return res;
1555 }
1556
setPedQuaternionRate(int64_t wanted)1557 int MPLSensor::setPedQuaternionRate(int64_t wanted)
1558 {
1559 VFUNC_LOG;
1560 int res = 0;
1561
1562 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1563 int(1000000000.f / wanted), mpu.ped_q_rate,
1564 getTimestamp());
1565 res = write_sysfs_int(mpu.ped_q_rate, 1000000000.f / wanted);
1566 LOGV_IF(PROCESS_VERBOSE,
1567 "HAL:DMP ped quaternion rate %.2f Hz", 1000000000.f / wanted);
1568
1569 return res;
1570 }
1571
check6AxisQuatEnabled(void)1572 int MPLSensor::check6AxisQuatEnabled(void)
1573 {
1574 VFUNC_LOG;
1575 return ((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)? 1:0);
1576 }
1577
1578 /* This is used for batch mode only */
1579 /* GRV is batched but not along with ped */
enable6AxisQuaternion(int en)1580 int MPLSensor::enable6AxisQuaternion(int en)
1581 {
1582 VFUNC_LOG;
1583
1584 if (!en) {
1585 enable6AxisQuaternionData(0);
1586 mFeatureActiveMask &= ~INV_DMP_6AXIS_QUATERNION;
1587 if (mFeatureActiveMask == 0) {
1588 onDmp(0);
1589 }
1590 LOGV_IF(ENG_VERBOSE, "HAL:6 Axis Quat disabled");
1591 } else {
1592 if (enable6AxisQuaternionData(1) < 0 || onDmp(1) < 0) {
1593 LOGE("HAL:ERR can't enable 6 Axis Quaternion");
1594 } else {
1595 mFeatureActiveMask |= INV_DMP_6AXIS_QUATERNION;
1596 LOGV_IF(PROCESS_VERBOSE, "HAL:6 Axis Quat enabled");
1597 }
1598 }
1599 return 0;
1600 }
1601
enable6AxisQuaternionData(int en)1602 int MPLSensor::enable6AxisQuaternionData(int en)
1603 {
1604 VFUNC_LOG;
1605
1606 int res = 0;
1607
1608 // Enable DMP quaternion
1609 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1610 en, mpu.six_axis_q_on, getTimestamp());
1611 if (write_sysfs_int(mpu.six_axis_q_on, en) < 0) {
1612 LOGE("HAL:ERR can't write DMP six_axis_q_on");
1613 res = -1; //Indicate an err
1614 }
1615
1616 if (!en) {
1617 LOGV_IF(EXTRA_VERBOSE, "HAL:DMP six axis quaternion data was turned off");
1618 inv_quaternion_sensor_was_turned_off();
1619 if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1620 && (!(mLocalSensorMask & mMasterSensorMask
1621 & INV_THREE_AXIS_ACCEL))) {
1622 res = enableAccel(0);
1623 if (res < 0)
1624 return res;
1625 }
1626 if (!(mFeatureActiveMask & DMP_FEATURE_MASK)
1627 && (!(mLocalSensorMask & mMasterSensorMask
1628 & INV_THREE_AXIS_GYRO))) {
1629 res = enableGyro(0);
1630 if (res < 0)
1631 return res;
1632 }
1633 if (mFeatureActiveMask & INV_DMP_QUATERNION) {
1634 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1635 1, mpu.gyro_fifo_enable, getTimestamp());
1636 res = write_sysfs_int(mpu.gyro_fifo_enable, 1);
1637 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1638 1, mpu.accel_fifo_enable, getTimestamp());
1639 res += write_sysfs_int(mpu.accel_fifo_enable, 1);
1640 if (res < 0)
1641 return res;
1642 }
1643 LOGV_IF(ENG_VERBOSE, " k=0x%lx", mLocalSensorMask);
1644 // reset global mask for buildMpuEvent()
1645 if (mEnabled & (1 << GameRotationVector)) {
1646 if (!(mFeatureActiveMask & INV_DMP_PED_QUATERNION)) {
1647 mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1648 mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1649 res = write_sysfs_int(mpu.gyro_fifo_enable, 1);
1650 res += write_sysfs_int(mpu.accel_fifo_enable, 1);
1651 if (res < 0)
1652 return res;
1653 }
1654 } else if (mEnabled & (1 << Accelerometer)) {
1655 mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
1656 } else if ((mEnabled & ( 1 << Gyro)) ||
1657 (mEnabled & (1 << RawGyro))) {
1658 mLocalSensorMask |= INV_THREE_AXIS_GYRO;
1659 }
1660 LOGV_IF(ENG_VERBOSE, "after mLocalSensorMask=0x%lx", mLocalSensorMask);
1661 } else {
1662 LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling six axis quat");
1663 if (mEnabled & ( 1 << GameRotationVector)) {
1664 // enable accel engine
1665 res = enableAccel(1);
1666 if (res < 0)
1667 return res;
1668
1669 // enable gyro engine
1670 res = enableGyro(1);
1671 if (res < 0)
1672 return res;
1673 LOGV_IF(EXTRA_VERBOSE, "before: mLocalSensorMask=0x%lx", mLocalSensorMask);
1674 if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) ||
1675 (!(mBatchEnabled & (1 << Accelerometer)) ||
1676 (!(mEnabled & (1 << Accelerometer))))) {
1677 res = turnOffAccelFifo();
1678 if (res < 0)
1679 return res;
1680 mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
1681 }
1682
1683 if ((!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_GYRO)) ||
1684 (!(mBatchEnabled & (1 << Gyro)) ||
1685 (!(mEnabled & (1 << Gyro))))) {
1686 if (!(mBatchEnabled & (1 << RawGyro)) ||
1687 (!(mEnabled & (1 << RawGyro)))) {
1688 res = turnOffGyroFifo();
1689 if (res < 0)
1690 return res;
1691 mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
1692 }
1693 }
1694 LOGV_IF(ENG_VERBOSE, "after: mLocalSensorMask=0x%lx", mLocalSensorMask);
1695 }
1696 }
1697
1698 return res;
1699 }
1700
set6AxisQuaternionRate(int64_t wanted)1701 int MPLSensor::set6AxisQuaternionRate(int64_t wanted)
1702 {
1703 VFUNC_LOG;
1704 int res = 0;
1705
1706 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1707 int(1000000000.f / wanted), mpu.six_axis_q_rate,
1708 getTimestamp());
1709 res = write_sysfs_int(mpu.six_axis_q_rate, 1000000000.f / wanted);
1710 LOGV_IF(PROCESS_VERBOSE,
1711 "HAL:DMP six axis rate %.2f Hz", 1000000000.f / wanted);
1712
1713 return res;
1714 }
1715
1716 /* this is for batch mode only */
checkLPQRateSupported(void)1717 int MPLSensor::checkLPQRateSupported(void)
1718 {
1719 VFUNC_LOG;
1720 #ifndef USE_LPQ_AT_FASTEST
1721 return ((mDelays[GameRotationVector] <= RATE_200HZ) ? 0 :1);
1722 #else
1723 return 1;
1724 #endif
1725 }
1726
checkLPQuaternion(void)1727 int MPLSensor::checkLPQuaternion(void)
1728 {
1729 VFUNC_LOG;
1730 return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0);
1731 }
1732
enableLPQuaternion(int en)1733 int MPLSensor::enableLPQuaternion(int en)
1734 {
1735 VFUNC_LOG;
1736
1737 if (!en) {
1738 enableQuaternionData(0);
1739 mFeatureActiveMask &= ~INV_DMP_QUATERNION;
1740 if (mFeatureActiveMask == 0) {
1741 onDmp(0);
1742 }
1743 LOGV_IF(ENG_VERBOSE, "HAL:LP Quat disabled");
1744 } else {
1745 if (enableQuaternionData(1) < 0 || onDmp(1) < 0) {
1746 LOGE("HAL:ERR can't enable LP Quaternion");
1747 } else {
1748 mFeatureActiveMask |= INV_DMP_QUATERNION;
1749 LOGV_IF(ENG_VERBOSE, "HAL:LP Quat enabled");
1750 }
1751 }
1752 return 0;
1753 }
1754
enableQuaternionData(int en)1755 int MPLSensor::enableQuaternionData(int en)
1756 {
1757 VFUNC_LOG;
1758
1759 int res = 0;
1760
1761 // Enable DMP quaternion
1762 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1763 en, mpu.three_axis_q_on, getTimestamp());
1764 if (write_sysfs_int(mpu.three_axis_q_on, en) < 0) {
1765 LOGE("HAL:ERR can't write DMP three_axis_q__on");
1766 res = -1; //Indicates an err
1767 }
1768
1769 if (!en) {
1770 LOGV_IF(ENG_VERBOSE, "HAL:DMP quaternion data was turned off");
1771 inv_quaternion_sensor_was_turned_off();
1772 } else {
1773 LOGV_IF(ENG_VERBOSE, "HAL:Enabling three axis quat");
1774 }
1775
1776 return res;
1777 }
1778
setQuaternionRate(int64_t wanted)1779 int MPLSensor::setQuaternionRate(int64_t wanted)
1780 {
1781 VFUNC_LOG;
1782 int res = 0;
1783
1784 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1785 int(1000000000.f / wanted), mpu.three_axis_q_rate,
1786 getTimestamp());
1787 res = write_sysfs_int(mpu.three_axis_q_rate, 1000000000.f / wanted);
1788 LOGV_IF(PROCESS_VERBOSE,
1789 "HAL:DMP three axis rate %.2f Hz", 1000000000.f / wanted);
1790
1791 return res;
1792 }
1793
enableDmpPedometer(int en,int interruptMode)1794 int MPLSensor::enableDmpPedometer(int en, int interruptMode)
1795 {
1796 VFUNC_LOG;
1797 int res = 0;
1798 int enabled_sensors = mEnabled;
1799
1800 if (isMpuNonDmp())
1801 return res;
1802
1803 // reset master enable
1804 res = masterEnable(0);
1805 if (res < 0) {
1806 return res;
1807 }
1808
1809 if (en == 1) {
1810 //Enable DMP Pedometer Function
1811 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1812 en, mpu.pedometer_on, getTimestamp());
1813 if (write_sysfs_int(mpu.pedometer_on, en) < 0) {
1814 LOGE("HAL:ERR can't enable Android Pedometer");
1815 res = -1; // indicate an err
1816 return res;
1817 }
1818
1819 if (interruptMode) {
1820 if(!checkPedStandaloneBatched()) {
1821 //Enable DMP Pedometer Interrupt
1822 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1823 en, mpu.pedometer_int_on, getTimestamp());
1824 if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) {
1825 LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
1826 res = -1; // indicate an err
1827 return res;
1828 }
1829 }
1830 }
1831
1832 if (interruptMode) {
1833 mFeatureActiveMask |= INV_DMP_PEDOMETER;
1834 }
1835 else {
1836 mFeatureActiveMask |= INV_DMP_PEDOMETER_STEP;
1837 }
1838
1839 mt_pre_ns = android::elapsedRealtimeNano();
1840 } else {
1841 if (interruptMode) {
1842 mFeatureActiveMask &= ~INV_DMP_PEDOMETER;
1843 }
1844 else {
1845 mFeatureActiveMask &= ~INV_DMP_PEDOMETER_STEP;
1846 mStepCountPollTime = -1;
1847 }
1848
1849 /* if neither step detector or step count is on */
1850 if (!(mFeatureActiveMask & (INV_DMP_PEDOMETER | INV_DMP_PEDOMETER_STEP))) {
1851 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1852 en, mpu.pedometer_on, getTimestamp());
1853 if (write_sysfs_int(mpu.pedometer_on, en) < 0) {
1854 LOGE("HAL:ERR can't enable Android Pedometer");
1855 res = -1;
1856 return res;
1857 }
1858 }
1859
1860 /* if feature is not step detector */
1861 if (!(mFeatureActiveMask & INV_DMP_PEDOMETER)) {
1862 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1863 en, mpu.pedometer_int_on, getTimestamp());
1864 if (write_sysfs_int(mpu.pedometer_int_on, en) < 0) {
1865 LOGE("HAL:ERR can't enable Android Pedometer Interrupt");
1866 res = -1;
1867 return res;
1868 }
1869 }
1870 }
1871
1872 if ((res = setDmpFeature(en)) < 0)
1873 return res;
1874
1875 if ((res = computeAndSetDmpState()) < 0)
1876 return res;
1877
1878 if (!mBatchEnabled && (resetDataRates() < 0))
1879 return res;
1880
1881 if(en || enabled_sensors || mFeatureActiveMask) {
1882 res = masterEnable(1);
1883 }
1884 return res;
1885 }
1886
masterEnable(int en)1887 int MPLSensor::masterEnable(int en)
1888 {
1889 VFUNC_LOG;
1890
1891 int res = 0;
1892 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1893 en, mpu.master_enable, getTimestamp());
1894 res = write_sysfs_int(mpu.master_enable, en);
1895 return res;
1896 }
1897
enableGyro(int en)1898 int MPLSensor::enableGyro(int en)
1899 {
1900 VFUNC_LOG;
1901
1902 int res = 0;
1903
1904 /* need to also turn on/off the master enable */
1905 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1906 en, mpu.gyro_enable, getTimestamp());
1907 res = write_sysfs_int(mpu.gyro_enable, en);
1908 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1909 en, mpu.gyro_fifo_enable, getTimestamp());
1910 res += write_sysfs_int(mpu.gyro_fifo_enable, en);
1911
1912 if (!en) {
1913 LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
1914 inv_gyro_was_turned_off();
1915 }
1916
1917 return res;
1918 }
1919
enableLowPowerAccel(int en)1920 int MPLSensor::enableLowPowerAccel(int en)
1921 {
1922 VFUNC_LOG;
1923
1924 int res;
1925
1926 /* need to also turn on/off the master enable */
1927 res = write_sysfs_int(mpu.motion_lpa_on, en);
1928 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1929 en, mpu.motion_lpa_on, getTimestamp());
1930 return res;
1931 }
1932
enableAccel(int en)1933 int MPLSensor::enableAccel(int en)
1934 {
1935 VFUNC_LOG;
1936
1937 int res;
1938
1939 /* need to also turn on/off the master enable */
1940 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1941 en, mpu.accel_enable, getTimestamp());
1942 res = write_sysfs_int(mpu.accel_enable, en);
1943 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
1944 en, mpu.accel_fifo_enable, getTimestamp());
1945 res += write_sysfs_int(mpu.accel_fifo_enable, en);
1946
1947 if (!en) {
1948 LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
1949 inv_accel_was_turned_off();
1950 }
1951
1952 return res;
1953 }
1954
enableCompass(int en,int rawSensorRequested)1955 int MPLSensor::enableCompass(int en, int rawSensorRequested)
1956 {
1957 VFUNC_LOG;
1958
1959 int res = 0;
1960 /* TODO: handle ID_RM if third party compass cal is used */
1961 if (rawSensorRequested && mCompassSensor->providesCalibration()) {
1962 res = mCompassSensor->enable(ID_RM, en);
1963 } else {
1964 res = mCompassSensor->enable(ID_M, en);
1965 }
1966 if (en == 0 || res != 0) {
1967 LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off %d", res);
1968 inv_compass_was_turned_off();
1969 }
1970
1971 return res;
1972 }
1973
1974 #ifdef ENABLE_PRESSURE
enablePressure(int en)1975 int MPLSensor::enablePressure(int en)
1976 {
1977 VFUNC_LOG;
1978
1979 int res = 0;
1980
1981 if (mPressureSensor)
1982 res = mPressureSensor->enable(ID_PS, en);
1983
1984 return res;
1985 }
1986 #endif
1987
1988 /* use this function for initialization */
enableBatch(int64_t timeout)1989 int MPLSensor::enableBatch(int64_t timeout)
1990 {
1991 VFUNC_LOG;
1992
1993 int res = 0;
1994
1995 res = write_sysfs_int(mpu.batchmode_timeout, timeout);
1996 if (timeout == 0) {
1997 res = write_sysfs_int(mpu.six_axis_q_on, 0);
1998 res = write_sysfs_int(mpu.ped_q_on, 0);
1999 res = write_sysfs_int(mpu.step_detector_on, 0);
2000 res = write_sysfs_int(mpu.step_indicator_on, 0);
2001 }
2002
2003 if (timeout == 0) {
2004 LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:batchmode timeout is zero");
2005 }
2006
2007 return res;
2008 }
2009
computeLocalSensorMask(int enabled_sensors)2010 void MPLSensor::computeLocalSensorMask(int enabled_sensors)
2011 {
2012 VFUNC_LOG;
2013
2014 do {
2015 #ifdef ENABLE_PRESSURE
2016 /* Invensense Pressure on secondary bus */
2017 if (PS_ENABLED) {
2018 LOGV_IF(ENG_VERBOSE, "PS ENABLED");
2019 mLocalSensorMask |= INV_ONE_AXIS_PRESSURE;
2020 } else {
2021 LOGV_IF(ENG_VERBOSE, "PS DISABLED");
2022 mLocalSensorMask &= ~INV_ONE_AXIS_PRESSURE;
2023 }
2024 #else
2025 LOGV_IF(ENG_VERBOSE, "PS DISABLED");
2026 mLocalSensorMask &= ~INV_ONE_AXIS_PRESSURE;
2027 #endif
2028
2029 if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED
2030 || (GRV_ENABLED && GMRV_ENABLED)) {
2031 LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
2032 mLocalSensorMask |= ALL_MPL_SENSORS_NP;
2033 break;
2034 }
2035
2036 if (GRV_ENABLED) {
2037 if (!(mFeatureActiveMask & INV_DMP_BATCH_MODE) ||
2038 !(mBatchEnabled & (1 << GameRotationVector))) {
2039 LOGV_IF(ENG_VERBOSE, "6 Axis Fusion ENABLED");
2040 mLocalSensorMask |= INV_THREE_AXIS_GYRO;
2041 mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
2042 } else {
2043 if (GY_ENABLED || RGY_ENABLED) {
2044 LOGV_IF(ENG_VERBOSE, "G ENABLED");
2045 mLocalSensorMask |= INV_THREE_AXIS_GYRO;
2046 } else {
2047 LOGV_IF(ENG_VERBOSE, "G DISABLED");
2048 mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
2049 }
2050 if (A_ENABLED) {
2051 LOGV_IF(ENG_VERBOSE, "A ENABLED");
2052 mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
2053 } else {
2054 LOGV_IF(ENG_VERBOSE, "A DISABLED");
2055 mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
2056 }
2057 }
2058 /* takes care of MAG case */
2059 if (M_ENABLED || RM_ENABLED) {
2060 LOGV_IF(1, "M ENABLED");
2061 mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
2062 } else {
2063 LOGV_IF(1, "M DISABLED");
2064 mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
2065 }
2066 break;
2067 }
2068
2069 if (GMRV_ENABLED) {
2070 LOGV_IF(ENG_VERBOSE, "6 Axis Geomagnetic Fusion ENABLED");
2071 mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
2072 mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
2073
2074 /* takes care of Gyro case */
2075 if (GY_ENABLED || RGY_ENABLED) {
2076 LOGV_IF(1, "G ENABLED");
2077 mLocalSensorMask |= INV_THREE_AXIS_GYRO;
2078 } else {
2079 LOGV_IF(1, "G DISABLED");
2080 mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
2081 }
2082 break;
2083 }
2084
2085 #ifdef ENABLE_PRESSURE
2086 if(!A_ENABLED && !M_ENABLED && !RM_ENABLED &&
2087 !GRV_ENABLED && !GMRV_ENABLED && !GY_ENABLED && !RGY_ENABLED &&
2088 !PS_ENABLED) {
2089 #else
2090 if(!A_ENABLED && !M_ENABLED && !RM_ENABLED &&
2091 !GRV_ENABLED && !GMRV_ENABLED && !GY_ENABLED && !RGY_ENABLED) {
2092 #endif
2093 /* Invensense compass cal */
2094 LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
2095 mLocalSensorMask = 0;
2096 break;
2097 }
2098
2099 if (GY_ENABLED || RGY_ENABLED) {
2100 LOGV_IF(ENG_VERBOSE, "G ENABLED");
2101 mLocalSensorMask |= INV_THREE_AXIS_GYRO;
2102 } else {
2103 LOGV_IF(ENG_VERBOSE, "G DISABLED");
2104 mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
2105 }
2106
2107 if (A_ENABLED) {
2108 LOGV_IF(ENG_VERBOSE, "A ENABLED");
2109 mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
2110 } else {
2111 LOGV_IF(ENG_VERBOSE, "A DISABLED");
2112 mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
2113 }
2114
2115 /* Invensense compass calibration */
2116 if (M_ENABLED || RM_ENABLED) {
2117 LOGV_IF(ENG_VERBOSE, "M ENABLED");
2118 mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
2119 } else {
2120 LOGV_IF(ENG_VERBOSE, "M DISABLED");
2121 mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
2122 }
2123 } while (0);
2124 }
2125
2126 int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed)
2127 {
2128 VFUNC_LOG;
2129
2130 inv_error_t res = -1;
2131 int on = 1;
2132 int off = 0;
2133 int cal_stored = 0;
2134
2135 // Sequence to enable or disable a sensor
2136 // 1. reset master enable (=0)
2137 // 2. enable or disable a sensor
2138 // 3. set master enable (=1)
2139
2140 if (isLowPowerQuatEnabled() ||
2141 changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
2142 (mCompassSensor->isIntegrated() << MagneticField) |
2143 #ifdef ENABLE_PRESSURE
2144 (mPressureSensor->isIntegrated() << Pressure) |
2145 #endif
2146 (mCompassSensor->isIntegrated() << RawMagneticField))) {
2147
2148 /* reset master enable */
2149 res = masterEnable(0);
2150 if(res < 0) {
2151 return res;
2152 }
2153 }
2154
2155 LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - sensors: 0x%0x",
2156 (unsigned int)sensors);
2157
2158 if (changed & ((1 << Gyro) | (1 << RawGyro))) {
2159 LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - gyro %s",
2160 (sensors & INV_THREE_AXIS_GYRO? "enable": "disable"));
2161 res = enableGyro(!!(sensors & INV_THREE_AXIS_GYRO));
2162 if(res < 0) {
2163 return res;
2164 }
2165
2166 if (!cal_stored && (!en && (changed & (1 << Gyro)))) {
2167 storeCalibration();
2168 cal_stored = 1;
2169 }
2170 }
2171
2172 if (changed & (1 << Accelerometer)) {
2173 LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - accel %s",
2174 (sensors & INV_THREE_AXIS_ACCEL? "enable": "disable"));
2175 res = enableAccel(!!(sensors & INV_THREE_AXIS_ACCEL));
2176 if(res < 0) {
2177 return res;
2178 }
2179
2180 if (!(sensors & INV_THREE_AXIS_ACCEL) && !cal_stored) {
2181 storeCalibration();
2182 cal_stored = 1;
2183 }
2184 }
2185
2186 if (changed & ((1 << MagneticField) | (1 << RawMagneticField))) {
2187 LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - compass %s",
2188 (sensors & INV_THREE_AXIS_COMPASS? "enable": "disable"));
2189 res = enableCompass(!!(sensors & INV_THREE_AXIS_COMPASS), changed & (1 << RawMagneticField));
2190 if(res < 0) {
2191 return res;
2192 }
2193
2194 if (!cal_stored && (!en && (changed & (1 << MagneticField)))) {
2195 storeCalibration();
2196 cal_stored = 1;
2197 }
2198 }
2199
2200 #ifdef ENABLE_PRESSURE
2201 if (changed & (1 << Pressure)) {
2202 LOGV_IF(ENG_VERBOSE, "HAL:enableSensors - pressure %s",
2203 (sensors & INV_ONE_AXIS_PRESSURE? "enable": "disable"));
2204 res = enablePressure(!!(sensors & INV_ONE_AXIS_PRESSURE));
2205 if(res < 0) {
2206 return res;
2207 }
2208 }
2209 #endif
2210
2211 if (isLowPowerQuatEnabled()) {
2212 // Enable LP Quat
2213 if ((mEnabled & VIRTUAL_SENSOR_9AXES_MASK)
2214 || (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK)) {
2215 LOGV_IF(ENG_VERBOSE, "HAL: 9 axis or game rot enabled");
2216 if (!(changed & ((1 << Gyro)
2217 | (1 << RawGyro)
2218 | (1 << Accelerometer)
2219 | (mCompassSensor->isIntegrated() << MagneticField)
2220 | (mCompassSensor->isIntegrated() << RawMagneticField)))
2221 ) {
2222 /* reset master enable */
2223 res = masterEnable(0);
2224 if(res < 0) {
2225 return res;
2226 }
2227 }
2228 if (!checkLPQuaternion()) {
2229 enableLPQuaternion(1);
2230 } else {
2231 LOGV_IF(ENG_VERBOSE, "HAL:LP Quat already enabled");
2232 }
2233 } else if (checkLPQuaternion()) {
2234 enableLPQuaternion(0);
2235 }
2236 }
2237
2238 /* apply accel/gyro bias to DMP bias */
2239 /* precondition: masterEnable(0), mGyroBiasAvailable=true */
2240 /* postcondition: bias is applied upon masterEnable(1) */
2241 if(!(sensors & INV_THREE_AXIS_GYRO)) {
2242 setGyroBias();
2243 }
2244 if(!(sensors & INV_THREE_AXIS_ACCEL)) {
2245 setAccelBias();
2246 }
2247
2248 /* to batch or not to batch */
2249 int batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled);
2250 /* skip setBatch if there is no need to */
2251 if(((int)mOldBatchEnabledMask != batchMode) || batchMode) {
2252 setBatch(batchMode,0);
2253 }
2254 mOldBatchEnabledMask = batchMode;
2255
2256 /* check for invn hardware sensors change */
2257 if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
2258 (mCompassSensor->isIntegrated() << MagneticField) |
2259 #ifdef ENABLE_PRESSURE
2260 (mPressureSensor->isIntegrated() << Pressure) |
2261 #endif
2262 (mCompassSensor->isIntegrated() << RawMagneticField))) {
2263 LOGV_IF(ENG_VERBOSE,
2264 "HAL DEBUG: Gyro, Accel, Compass, Pressure changes");
2265 if ((checkSmdSupport() == 1) || (checkPedometerSupport() == 1) || (sensors &
2266 (INV_THREE_AXIS_GYRO
2267 | INV_THREE_AXIS_ACCEL
2268 #ifdef ENABLE_PRESSURE
2269 | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())
2270 #endif
2271 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated())))) {
2272 LOGV_IF(ENG_VERBOSE, "SMD or Hardware sensors enabled");
2273 LOGV_IF(ENG_VERBOSE,
2274 "mFeatureActiveMask=0x%llx", mFeatureActiveMask);
2275 LOGV_IF(ENG_VERBOSE, "HAL DEBUG: LPQ, SMD, SO enabled");
2276 // disable DMP event interrupt only (w/ data interrupt)
2277 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
2278 0, mpu.dmp_event_int_on, getTimestamp());
2279 if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
2280 res = -1;
2281 LOGE("HAL:ERR can't disable DMP event interrupt");
2282 return res;
2283 }
2284 LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=0x%llx", mFeatureActiveMask);
2285 LOGV_IF(ENG_VERBOSE, "DMP_FEATURE_MASK=0x%x", DMP_FEATURE_MASK);
2286 if ((mFeatureActiveMask & (long long)DMP_FEATURE_MASK) &&
2287 !((mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) ||
2288 (mFeatureActiveMask & INV_DMP_PED_STANDALONE) ||
2289 (mFeatureActiveMask & INV_DMP_PED_QUATERNION) ||
2290 (mFeatureActiveMask & INV_DMP_BATCH_MODE))) {
2291 // enable DMP
2292 onDmp(1);
2293 res = enableAccel(on);
2294 if(res < 0) {
2295 return res;
2296 }
2297 LOGV_IF(ENG_VERBOSE, "mLocalSensorMask=0x%lx", mLocalSensorMask);
2298 if (((sensors | mLocalSensorMask) & INV_THREE_AXIS_ACCEL) == 0) {
2299 res = turnOffAccelFifo();
2300 }
2301 if(res < 0) {
2302 return res;
2303 }
2304 }
2305 } else { // all sensors idle
2306 LOGV_IF(ENG_VERBOSE, "HAL DEBUG: not SMD or Hardware sensors");
2307 if (isDmpDisplayOrientationOn()
2308 && (mDmpOrientationEnabled
2309 || !isDmpScreenAutoRotationEnabled())) {
2310 enableDmpOrientation(1);
2311 }
2312
2313 if (!cal_stored) {
2314 storeCalibration();
2315 cal_stored = 1;
2316 }
2317 }
2318 } else if ((changed &
2319 ((!mCompassSensor->isIntegrated()) << MagneticField) ||
2320 ((!mCompassSensor->isIntegrated()) << RawMagneticField))
2321 &&
2322 !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
2323 | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))
2324 ) {
2325 LOGV_IF(ENG_VERBOSE, "HAL DEBUG: Gyro, Accel, Compass no change");
2326 if (!cal_stored) {
2327 storeCalibration();
2328 cal_stored = 1;
2329 }
2330 } /*else {
2331 LOGV_IF(ENG_VERBOSE, "HAL DEBUG: mEnabled");
2332 if (sensors &
2333 (INV_THREE_AXIS_GYRO
2334 | INV_THREE_AXIS_ACCEL
2335 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
2336 res = masterEnable(1);
2337 if(res < 0)
2338 return res;
2339 }
2340 }*/
2341
2342 if (!batchMode && (resetDataRates() < 0)) {
2343 LOGE("HAL:ERR can't reset output rate back to original setting");
2344 }
2345
2346 if(mFeatureActiveMask || sensors) {
2347 res = masterEnable(1);
2348 if(res < 0)
2349 return res;
2350 }
2351 return res;
2352 }
2353
2354 /* check if batch mode should be turned on or not */
2355 int MPLSensor::computeBatchSensorMask(int enableSensors, int tempBatchSensor)
2356 {
2357 VFUNC_LOG;
2358 int batchMode = 1;
2359 mFeatureActiveMask &= ~INV_DMP_BATCH_MODE;
2360
2361 LOGV_IF(ENG_VERBOSE,
2362 "HAL:computeBatchSensorMask: enableSensors=%d tempBatchSensor=%d",
2363 enableSensors, tempBatchSensor);
2364
2365 // handle initialization case
2366 if (enableSensors == 0 && tempBatchSensor == 0)
2367 return 0;
2368
2369 // check for possible continuous data mode
2370 for(int i = 0; i <= LAST_HW_SENSOR; i++) {
2371 // if any one of the hardware sensor is in continuous data mode
2372 // turn off batch mode.
2373 if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) {
2374 LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: "
2375 "hardware sensor on continuous mode:%d", i);
2376 return 0;
2377 }
2378 if ((enableSensors & (1 << i)) && (tempBatchSensor & (1 << i))) {
2379 LOGV_IF(ENG_VERBOSE,
2380 "HAL:computeBatchSensorMask: hardware sensor is batch:%d",
2381 i);
2382 // if hardware sensor is batched, check if virtual sensor is also batched
2383 if ((enableSensors & (1 << GameRotationVector))
2384 && !(tempBatchSensor & (1 << GameRotationVector))) {
2385 LOGV_IF(ENG_VERBOSE,
2386 "HAL:computeBatchSensorMask: but virtual sensor is not:%d",
2387 i);
2388 return 0;
2389 }
2390 }
2391 }
2392
2393 // if virtual sensors are on but not batched, turn off batch mode.
2394 for(int i = Orientation; i < NumSensors; i++) {
2395 if ((enableSensors & (1 << i)) && !(tempBatchSensor & (1 << i))) {
2396 LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: "
2397 "composite sensor on continuous mode:%d", i);
2398 return 0;
2399 }
2400 }
2401
2402 if ((mFeatureActiveMask & INV_DMP_PEDOMETER) && !(tempBatchSensor & (1 << StepDetector))) {
2403 LOGV_IF(ENG_VERBOSE, "HAL:computeBatchSensorMask: step detector on continuous mode.");
2404 return 0;
2405 }
2406
2407 mFeatureActiveMask |= INV_DMP_BATCH_MODE;
2408 LOGV_IF(EXTRA_VERBOSE,
2409 "HAL:computeBatchSensorMask: batchMode=%d, mBatchEnabled=%0x",
2410 batchMode, tempBatchSensor);
2411 return (batchMode && tempBatchSensor);
2412 }
2413
2414 /* This function is called by enable() */
2415 int MPLSensor::setBatch(int en, int toggleEnable)
2416 {
2417 VFUNC_LOG;
2418
2419 int res = 0;
2420 int64_t wanted = 1000000000LL;
2421 int64_t timeout = 0;
2422 int timeoutInMs = 0;
2423 int featureMask = computeBatchDataOutput();
2424
2425 // reset master enable
2426 if (toggleEnable == 1) {
2427 res = masterEnable(0);
2428 if (res < 0) {
2429 return res;
2430 }
2431 }
2432
2433 /* step detector is enabled and */
2434 /* batch mode is standalone */
2435 if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) &&
2436 (featureMask & INV_DMP_PED_STANDALONE)) {
2437 LOGV_IF(ENG_VERBOSE, "setBatch: ID_P only = 0x%x", mBatchEnabled);
2438 enablePedStandalone(1);
2439 } else {
2440 enablePedStandalone(0);
2441 }
2442
2443 /* step detector and GRV are enabled and */
2444 /* batch mode is ped q */
2445 if (en && (mFeatureActiveMask & INV_DMP_PEDOMETER) &&
2446 (mEnabled & (1 << GameRotationVector)) &&
2447 (featureMask & INV_DMP_PED_QUATERNION)) {
2448 LOGV_IF(ENG_VERBOSE, "setBatch: ID_P and GRV or ALL = 0x%x", mBatchEnabled);
2449 LOGV_IF(ENG_VERBOSE, "setBatch: ID_P is enabled for batching, "
2450 "PED quat will be automatically enabled");
2451 enableLPQuaternion(0);
2452 enablePedQuaternion(1);
2453 } else if (!(featureMask & INV_DMP_PED_STANDALONE)){
2454 enablePedQuaternion(0);
2455 } else {
2456 enablePedQuaternion(0);
2457 }
2458
2459 /* step detector and hardware sensors enabled */
2460 if (en && (featureMask & INV_DMP_PED_INDICATOR) &&
2461 ((mEnabled) ||
2462 (mFeatureActiveMask & INV_DMP_PED_STANDALONE))) {
2463 enablePedIndicator(1);
2464 } else {
2465 enablePedIndicator(0);
2466 }
2467
2468 /* GRV is enabled and */
2469 /* batch mode is 6axis q */
2470 if (en && (mEnabled & (1 << GameRotationVector)) &&
2471 (featureMask & INV_DMP_6AXIS_QUATERNION)) {
2472 LOGV_IF(ENG_VERBOSE, "setBatch: GRV = 0x%x", mBatchEnabled);
2473 enableLPQuaternion(0);
2474 enable6AxisQuaternion(1);
2475 setInitial6QuatValue();
2476 } else if (!(featureMask & INV_DMP_PED_QUATERNION)){
2477 LOGV_IF(ENG_VERBOSE, "setBatch: Toggle back to normal 6 axis");
2478 if (mEnabled & (1 << GameRotationVector)) {
2479 enableLPQuaternion(checkLPQRateSupported());
2480 }
2481 enable6AxisQuaternion(0);
2482 } else {
2483 enable6AxisQuaternion(0);
2484 }
2485
2486 writeBatchTimeout(en);
2487
2488 if (en) {
2489 // enable DMP
2490 res = onDmp(1);
2491 if (res < 0) {
2492 return res;
2493 }
2494
2495 // set batch rates
2496 if (setBatchDataRates() < 0) {
2497 LOGE("HAL:ERR can't set batch data rates");
2498 }
2499
2500 // default fifo rate to 200Hz
2501 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
2502 200, mpu.gyro_fifo_rate, getTimestamp());
2503 if (write_sysfs_int(mpu.gyro_fifo_rate, 200) < 0) {
2504 res = -1;
2505 LOGE("HAL:ERR can't set rate to 200Hz");
2506 return res;
2507 }
2508 } else {
2509 if (mFeatureActiveMask == 0) {
2510 // disable DMP
2511 res = onDmp(0);
2512 if (res < 0) {
2513 return res;
2514 }
2515 /* reset sensor rate */
2516 if (resetDataRates() < 0) {
2517 LOGE("HAL:ERR can't reset output rate back to original setting");
2518 }
2519 }
2520 }
2521
2522 // set sensor data interrupt
2523 uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE));
2524 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
2525 !dataInterrupt, mpu.dmp_event_int_on, getTimestamp());
2526 if (write_sysfs_int(mpu.dmp_event_int_on, !dataInterrupt) < 0) {
2527 res = -1;
2528 LOGE("HAL:ERR can't enable DMP event interrupt");
2529 }
2530
2531 if (toggleEnable == 1) {
2532 if (mFeatureActiveMask || mEnabled)
2533 masterEnable(1);
2534 }
2535 return res;
2536 }
2537
2538 int MPLSensor::calcBatchTimeout(int en, int64_t *out)
2539 {
2540 VFUNC_LOG;
2541
2542 int64_t timeoutInMs = 0;
2543 if (en) {
2544 /* take the minimum batchmode timeout */
2545 int64_t timeout = 100000000000LL;
2546 int64_t ns = 0;
2547 for (int i = 0; i < NumSensors; i++) {
2548 LOGV_IF(0, "mFeatureActiveMask=0x%016llx, mEnabled=0x%01x, mBatchEnabled=0x%x",
2549 mFeatureActiveMask, mEnabled, mBatchEnabled);
2550 if (((mEnabled & (1 << i)) && (mBatchEnabled & (1 << i))) ||
2551 (checkPedStandaloneBatched() && (i == StepDetector))) {
2552 LOGV_IF(ENG_VERBOSE, "sensor=%d, timeout=%lld", i, mBatchTimeouts[i]);
2553 ns = mBatchTimeouts[i];
2554 timeout = (ns < timeout) ? ns : timeout;
2555 }
2556 }
2557 /* Convert ns to millisecond */
2558 timeoutInMs = timeout / 1000000;
2559 } else {
2560 timeoutInMs = 0;
2561 }
2562
2563 *out = timeoutInMs;
2564
2565 return 0;
2566 }
2567
2568 int MPLSensor::writeBatchTimeout(int en, int64_t timeoutInMs)
2569 {
2570 VFUNC_LOG;
2571
2572 if(mBatchTimeoutInMs != timeoutInMs) {
2573 /* write required timeout to sysfs */
2574 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %lld > %s (%lld)",
2575 timeoutInMs, mpu.batchmode_timeout, getTimestamp());
2576 if (write_sysfs_int(mpu.batchmode_timeout, timeoutInMs) < 0) {
2577 LOGE("HAL:ERR can't write batchmode_timeout");
2578 }
2579 }
2580 /* remember last timeout value */
2581 mBatchTimeoutInMs = timeoutInMs;
2582
2583 return 0;
2584 }
2585
2586 int MPLSensor::writeBatchTimeout(int en)
2587 {
2588 VFUNC_LOG;
2589
2590 int64_t timeoutInMs = 0;
2591
2592 calcBatchTimeout(en, &timeoutInMs);
2593 LOGV_IF(PROCESS_VERBOSE,
2594 "HAL: batch timeout set to %lld ms", timeoutInMs);
2595
2596 writeBatchTimeout(en, timeoutInMs);
2597
2598 return 0;
2599 }
2600
2601 /* Store calibration file */
2602 void MPLSensor::storeCalibration(void)
2603 {
2604 VFUNC_LOG;
2605
2606 if(mHaveGoodMpuCal == true
2607 || mAccelAccuracy >= 2
2608 || mCompassAccuracy >= 3) {
2609 int res = inv_store_calibration();
2610 if (res) {
2611 LOGE("HAL:Cannot store calibration on file");
2612 } else {
2613 LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated");
2614 }
2615 }
2616 }
2617
2618 /* these handlers transform mpl data into one of the Android sensor types */
2619 int MPLSensor::gyroHandler(sensors_event_t* s)
2620 {
2621 VHANDLER_LOG;
2622 int update;
2623 #if defined ANDROID_LOLLIPOP
2624 update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status,
2625 (inv_time_t *)(&s->timestamp));
2626 #else
2627 update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status,
2628 &s->timestamp);
2629 #endif
2630 if (!mEnabledTime[Gyro] || !(s->timestamp > mEnabledTime[Gyro])) {
2631 LOGV_IF(ENG_VERBOSE, "HAL:gyro incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2632 mEnabledTime[Gyro], s->timestamp, android::elapsedRealtimeNano());
2633 update = 0;
2634 }
2635
2636 LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
2637 s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
2638 return update;
2639 }
2640
2641 int MPLSensor::rawGyroHandler(sensors_event_t* s)
2642 {
2643 VHANDLER_LOG;
2644 int update;
2645 #if defined ANDROID_LOLLIPOP
2646 update = inv_get_sensor_type_gyroscope_raw(s->uncalibrated_gyro.uncalib,
2647 &s->gyro.status, (inv_time_t *)(&s->timestamp));
2648 #else
2649 update = inv_get_sensor_type_gyroscope_raw(s->uncalibrated_gyro.uncalib,
2650 &s->gyro.status, &s->timestamp);
2651 #endif
2652 if (!mEnabledTime[RawGyro] || !(s->timestamp > mEnabledTime[RawGyro])) {
2653 LOGV_IF(ENG_VERBOSE, "HAL:raw gyro incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2654 mEnabledTime[RawGyro], s->timestamp, android::elapsedRealtimeNano());
2655 update = 0;
2656 }
2657
2658 if(update) {
2659 memcpy(s->uncalibrated_gyro.bias, mGyroBias, sizeof(mGyroBias));
2660 LOGV_IF(HANDLER_DATA,"HAL:gyro bias data : %+f %+f %+f -- %lld - %d",
2661 s->uncalibrated_gyro.bias[0], s->uncalibrated_gyro.bias[1],
2662 s->uncalibrated_gyro.bias[2], s->timestamp, update);
2663 }
2664 s->gyro.status = SENSOR_STATUS_UNRELIABLE;
2665 LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
2666 s->uncalibrated_gyro.uncalib[0], s->uncalibrated_gyro.uncalib[1],
2667 s->uncalibrated_gyro.uncalib[2], s->timestamp, update);
2668 return update;
2669 }
2670
2671 int MPLSensor::accelHandler(sensors_event_t* s)
2672 {
2673 VHANDLER_LOG;
2674 int update;
2675 #if defined ANDROID_LOLLIPOP
2676 update = inv_get_sensor_type_accelerometer(
2677 s->acceleration.v, &s->acceleration.status, (inv_time_t *)(&s->timestamp));
2678 #else
2679 update = inv_get_sensor_type_accelerometer(
2680 s->acceleration.v, &s->acceleration.status, &s->timestamp);
2681 #endif
2682 if (!mEnabledTime[Accelerometer] || !(s->timestamp > mEnabledTime[Accelerometer])) {
2683 LOGV_IF(ENG_VERBOSE, "HAL:accel incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2684 mEnabledTime[Accelerometer], s->timestamp, android::elapsedRealtimeNano());
2685 update = 0;
2686 }
2687
2688 LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
2689 s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
2690 s->timestamp, update);
2691 mAccelAccuracy = s->acceleration.status;
2692 return update;
2693 }
2694
2695 int MPLSensor::compassHandler(sensors_event_t* s)
2696 {
2697 VHANDLER_LOG;
2698 int update;
2699 int overflow = mCompassOverFlow;
2700 #if defined ANDROID_LOLLIPOP
2701 update = inv_get_sensor_type_magnetic_field(
2702 s->magnetic.v, &s->magnetic.status, (inv_time_t *)(&s->timestamp));
2703 #else
2704 update = inv_get_sensor_type_magnetic_field(
2705 s->magnetic.v, &s->magnetic.status, &s->timestamp);
2706 #endif
2707 if (!mEnabledTime[MagneticField] || !(s->timestamp > mEnabledTime[MagneticField])) {
2708 LOGV_IF(ENG_VERBOSE, "HAL:compass incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2709 mEnabledTime[MagneticField], s->timestamp, android::elapsedRealtimeNano());
2710 overflow = 0;
2711 update = 0;
2712 }
2713 LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
2714 s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2],
2715 s->timestamp, update);
2716 mCompassAccuracy = s->magnetic.status;
2717 return update | overflow;
2718 }
2719
2720 int MPLSensor::rawCompassHandler(sensors_event_t* s)
2721 {
2722 VHANDLER_LOG;
2723 int update;
2724 int overflow = mCompassOverFlow;
2725 //TODO: need to handle uncalib data and bias for 3rd party compass
2726 #if defined ANDROID_LOLLIPOP
2727 if(mCompassSensor->providesCalibration()) {
2728 update = mCompassSensor->readRawSample(s->uncalibrated_magnetic.uncalib, (int64_t *)(&s->timestamp));
2729 }
2730 else {
2731 update = inv_get_sensor_type_magnetic_field_raw(s->uncalibrated_magnetic.uncalib,
2732 &s->magnetic.status, (inv_time_t *)(&s->timestamp));
2733 }
2734 #else
2735 if(mCompassSensor->providesCalibration()) {
2736 update = mCompassSensor->readRawSample(s->uncalibrated_magnetic.uncalib, &s->timestamp);
2737 }
2738 else {
2739 update = inv_get_sensor_type_magnetic_field_raw(s->uncalibrated_magnetic.uncalib,
2740 &s->magnetic.status, &s->timestamp);
2741 }
2742 #endif
2743 if (!mEnabledTime[RawMagneticField] || !(s->timestamp > mEnabledTime[RawMagneticField])) {
2744 LOGV_IF(ENG_VERBOSE, "HAL:raw compass incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2745 mEnabledTime[RawMagneticField], s->timestamp, android::elapsedRealtimeNano());
2746 overflow = 0;
2747 update = 0;
2748 }
2749 if(update) {
2750 memcpy(s->uncalibrated_magnetic.bias, mCompassBias, sizeof(mCompassBias));
2751 LOGV_IF(HANDLER_DATA, "HAL:compass bias data: %+f %+f %+f -- %lld - %d",
2752 s->uncalibrated_magnetic.bias[0], s->uncalibrated_magnetic.bias[1],
2753 s->uncalibrated_magnetic.bias[2], s->timestamp, update);
2754 }
2755 s->magnetic.status = SENSOR_STATUS_UNRELIABLE;
2756 LOGV_IF(HANDLER_DATA, "HAL:compass raw data: %+f %+f %+f %d -- %lld - %d",
2757 s->uncalibrated_magnetic.uncalib[0], s->uncalibrated_magnetic.uncalib[1],
2758 s->uncalibrated_magnetic.uncalib[2], s->magnetic.status, s->timestamp, update);
2759 return update | overflow;
2760 }
2761
2762 /*
2763 Rotation Vector handler.
2764 NOTE: rotation vector does not have an accuracy or status
2765 */
2766 int MPLSensor::rvHandler(sensors_event_t* s)
2767 {
2768 VHANDLER_LOG;
2769 int8_t status;
2770 int update;
2771 #if defined ANDROID_LOLLIPOP
2772 update = inv_get_sensor_type_rotation_vector(s->data, &status,
2773 (inv_time_t *)(&s->timestamp));
2774 #else
2775 update = inv_get_sensor_type_rotation_vector(s->data, &status,
2776 &s->timestamp);
2777 #endif
2778 s->orientation.status = status;
2779
2780 update |= isCompassDisabled();
2781
2782 if (!mEnabledTime[RotationVector] || !(s->timestamp > mEnabledTime[RotationVector])) {
2783 LOGV_IF(ENG_VERBOSE, "HAL:rv incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2784 mEnabledTime[RotationVector], s->timestamp, android::elapsedRealtimeNano());
2785 update = 0;
2786 }
2787
2788 LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f %+f %d- %+lld - %d",
2789 s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->orientation.status, s->timestamp,
2790 update);
2791
2792 return update;
2793 }
2794
2795 /*
2796 Game Rotation Vector handler.
2797 NOTE: rotation vector does not have an accuracy or status
2798 */
2799 int MPLSensor::grvHandler(sensors_event_t* s)
2800 {
2801 VHANDLER_LOG;
2802 int8_t status;
2803 int update;
2804 #if defined ANDROID_LOLLIPOP
2805 update = inv_get_sensor_type_rotation_vector_6_axis(s->data, &status,
2806 (inv_time_t *)(&s->timestamp));
2807 #else
2808 update = inv_get_sensor_type_rotation_vector_6_axis(s->data, &status,
2809 &s->timestamp);
2810 #endif
2811 s->orientation.status = status;
2812
2813 if (!mEnabledTime[GameRotationVector] || !(s->timestamp > mEnabledTime[GameRotationVector])) {
2814 LOGV_IF(ENG_VERBOSE, "HAL:grv incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2815 mEnabledTime[GameRotationVector], s->timestamp, android::elapsedRealtimeNano());
2816 update = 0;
2817 }
2818
2819 LOGV_IF(HANDLER_DATA, "HAL:grv data: %+f %+f %+f %+f %+f %d- %+lld - %d",
2820 s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->orientation.status, s->timestamp,
2821 update);
2822 return update;
2823 }
2824
2825 int MPLSensor::laHandler(sensors_event_t* s)
2826 {
2827 VHANDLER_LOG;
2828 int update;
2829 #if defined ANDROID_LOLLIPOP
2830 update = inv_get_sensor_type_linear_acceleration(
2831 s->gyro.v, &s->gyro.status, (inv_time_t *)(&s->timestamp));
2832 #else
2833 update = inv_get_sensor_type_linear_acceleration(
2834 s->gyro.v, &s->gyro.status, &s->timestamp);
2835 #endif
2836 update |= isCompassDisabled();
2837
2838 if (!mEnabledTime[LinearAccel] || !(s->timestamp > mEnabledTime[LinearAccel])) {
2839 LOGV_IF(ENG_VERBOSE, "HAL:la incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2840 mEnabledTime[LinearAccel], s->timestamp, android::elapsedRealtimeNano());
2841 update = 0;
2842 }
2843
2844 LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
2845 s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
2846 return update;
2847 }
2848
2849 int MPLSensor::gravHandler(sensors_event_t* s)
2850 {
2851 VHANDLER_LOG;
2852 int update;
2853 #if defined ANDROID_LOLLIPOP
2854 update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status,
2855 (inv_time_t *)(&s->timestamp));
2856 #else
2857 update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status,
2858 &s->timestamp);
2859 #endif
2860 update |= isCompassDisabled();
2861
2862 if (!mEnabledTime[Gravity] || !(s->timestamp > mEnabledTime[Gravity])) {
2863 LOGV_IF(ENG_VERBOSE, "HAL:gr incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2864 mEnabledTime[Gravity], s->timestamp, android::elapsedRealtimeNano());
2865 update = 0;
2866 }
2867
2868 LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
2869 s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
2870 return update;
2871 }
2872
2873 int MPLSensor::orienHandler(sensors_event_t* s)
2874 {
2875 VHANDLER_LOG;
2876 int update;
2877 #if defined ANDROID_LOLLIPOP
2878 update = inv_get_sensor_type_orientation(
2879 s->orientation.v, &s->orientation.status, (inv_time_t *)(&s->timestamp));
2880 #else
2881 update = inv_get_sensor_type_orientation(
2882 s->orientation.v, &s->orientation.status, &s->timestamp);
2883
2884 #endif
2885 update |= isCompassDisabled();
2886
2887 if (!mEnabledTime[Orientation] || !(s->timestamp > mEnabledTime[Orientation])) {
2888 LOGV_IF(ENG_VERBOSE, "HAL:or incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2889 mEnabledTime[Orientation], s->timestamp, android::elapsedRealtimeNano());
2890 update = 0;
2891 }
2892
2893 LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
2894 s->orientation.v[0], s->orientation.v[1], s->orientation.v[2],
2895 s->timestamp, update);
2896 return update;
2897 }
2898
2899 int MPLSensor::smHandler(sensors_event_t* s)
2900 {
2901 VHANDLER_LOG;
2902 int update = 1;
2903
2904 /* When event is triggered, set data to 1 */
2905 s->data[0] = 1.f;
2906 s->data[1] = 0.f;
2907 s->data[2] = 0.f;
2908
2909 /* Capture timestamp in HAL */
2910 s->timestamp = android::elapsedRealtimeNano();
2911
2912 if (!mEnabledTime[SignificantMotion] || !(s->timestamp > mEnabledTime[SignificantMotion])) {
2913 LOGV_IF(ENG_VERBOSE, "HAL:sm incorrect timestamp Enabled=%lld, Timestamp=%lld",
2914 mEnabledTime[SignificantMotion], s->timestamp);
2915 update = 0;
2916 }
2917
2918 LOGV_IF(HANDLER_DATA, "HAL:sm data: %f - %lld - %d",
2919 s->data[0], s->timestamp, update);
2920 return update;
2921 }
2922
2923 int MPLSensor::gmHandler(sensors_event_t* s)
2924 {
2925 VHANDLER_LOG;
2926 int8_t status;
2927 int update = 0;
2928
2929 #if defined ANDROID_LOLLIPOP
2930 update = inv_get_sensor_type_geomagnetic_rotation_vector(s->data, &status,
2931 (inv_time_t *)(&s->timestamp));
2932 #else
2933 update = inv_get_sensor_type_geomagnetic_rotation_vector(s->data, &status,
2934 &s->timestamp);
2935 #endif
2936 s->orientation.status = status;
2937
2938 if (!mEnabledTime[GeomagneticRotationVector] || !(s->timestamp > mEnabledTime[GeomagneticRotationVector])) {
2939 LOGV_IF(ENG_VERBOSE, "HAL:gm incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2940 mEnabledTime[GeomagneticRotationVector], s->timestamp, android::elapsedRealtimeNano());
2941 update = 0;
2942 }
2943
2944 LOGV_IF(HANDLER_DATA, "HAL:gm data: %+f %+f %+f %+f %+f %d- %+lld - %d",
2945 s->data[0], s->data[1], s->data[2], s->data[3], s->data[4], s->orientation.status, s->timestamp, update);
2946 return update < 1 ? 0 :1;
2947
2948 }
2949
2950 int MPLSensor::psHandler(sensors_event_t* s)
2951 {
2952 VHANDLER_LOG;
2953 int8_t status;
2954 int update = 0;
2955
2956 s->pressure = mCachedPressureData / 100.f; //hpa (millibar)
2957 s->data[1] = 0;
2958 s->data[2] = 0;
2959 s->timestamp = mPressureTimestamp;
2960 s->magnetic.status = 0;
2961 update = mPressureUpdate;
2962 mPressureUpdate = 0;
2963
2964 #ifdef ENABLE_PRESSURE
2965 if (!mEnabledTime[Pressure] || !(s->timestamp > mEnabledTime[Pressure])) {
2966 LOGV_IF(ENG_VERBOSE, "HAL:ps incorrect timestamp Enabled=%lld, Timestamp=%lld, Now=%lld",
2967 mEnabledTime[Pressure], s->timestamp, android::elapsedRealtimeNano());
2968 update = 0;
2969 }
2970 #endif
2971
2972 LOGV_IF(HANDLER_DATA, "HAL:ps data: %+f %+f %+f %+f- %+lld - %d",
2973 s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
2974 return update < 1 ? 0 :1;
2975
2976 }
2977
2978 int MPLSensor::sdHandler(sensors_event_t* s)
2979 {
2980 VHANDLER_LOG;
2981 int update = 1;
2982
2983 /* When event is triggered, set data to 1 */
2984 s->data[0] = 1;
2985 s->data[1] = 0.f;
2986 s->data[2] = 0.f;
2987
2988 /* get current timestamp */
2989 s->timestamp = android::elapsedRealtimeNano();
2990
2991 LOGV_IF(HANDLER_DATA, "HAL:sd data: %f - %lld - %d",
2992 s->data[0], s->timestamp, update);
2993 return update;
2994 }
2995
2996 int MPLSensor::scHandler(sensors_event_t* s)
2997 {
2998 VHANDLER_LOG;
2999 int update = 1;
3000
3001 /* Set step count */
3002 #if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP
3003 s->u64.step_counter = mLastStepCount;
3004 LOGV_IF(HANDLER_DATA, "HAL:sc data: %lld - %lld - %d",
3005 s->u64.step_counter, s->timestamp, update);
3006 #else
3007 s->step_counter = mLastStepCount;
3008 LOGV_IF(HANDLER_DATA, "HAL:sc data: %lld - %lld - %d",
3009 s->step_counter, s->timestamp, update);
3010 #endif
3011
3012 if (s->timestamp == 0 && update) {
3013 s->timestamp = android::elapsedRealtimeNano();
3014 }
3015
3016 return update;
3017 }
3018
3019 int MPLSensor::metaHandler(sensors_event_t* s, int flags)
3020 {
3021 VHANDLER_LOG;
3022 int update = 1;
3023
3024 #if defined ANDROID_KITKAT || defined ANDROID_LOLLIPOP
3025 /* initalize SENSOR_TYPE_META_DATA */
3026 s->version = 0;
3027 s->sensor = 0;
3028 s->reserved0 = 0;
3029 s->timestamp = 0LL;
3030
3031 switch(flags) {
3032 case META_DATA_FLUSH_COMPLETE:
3033 s->type = SENSOR_TYPE_META_DATA;
3034 s->version = META_DATA_VERSION;
3035 s->meta_data.what = flags;
3036 s->meta_data.sensor = mFlushSensorEnabledVector[0];
3037 mFlushSensorEnabledVector.removeAt(0);
3038 LOGV_IF(HANDLER_DATA,
3039 "HAL:flush complete data: type=%d what=%d, "
3040 "sensor=%d - %lld - %d",
3041 s->type, s->meta_data.what, s->meta_data.sensor,
3042 s->timestamp, update);
3043 break;
3044
3045 default:
3046 LOGW("HAL: Meta flags not supported");
3047 break;
3048 }
3049 #endif
3050 return 1;
3051 }
3052
3053 int MPLSensor::enable(int32_t handle, int en)
3054 {
3055 VFUNC_LOG;
3056
3057 android::String8 sname;
3058 int what = -1, err = 0;
3059 int batchMode = 0;
3060
3061 if (uint32_t(handle) >= NumSensors)
3062 return -EINVAL;
3063
3064 /* set called flag */
3065 mEnableCalled = 1;
3066
3067 if (!en)
3068 mBatchEnabled &= ~(1 << handle);
3069
3070 LOGV_IF(ENG_VERBOSE, "HAL: MPLSensor::enable(handle = %d, en = %d)", handle, en);
3071
3072 switch (handle) {
3073 case ID_SC:
3074 what = StepCounter;
3075 sname = "Step Counter";
3076 LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
3077 sname.string(), handle,
3078 (mDmpStepCountEnabled? "en": "dis"),
3079 (en? "en" : "dis"));
3080 enableDmpPedometer(en, 0);
3081 mDmpStepCountEnabled = !!en;
3082 if (en)
3083 mEnabledTime[StepCounter] = android::elapsedRealtimeNano();
3084 else
3085 mEnabledTime[StepCounter] = 0;
3086
3087 if (!en)
3088 mBatchDelays[what] = 1000000000LL;
3089 return 0;
3090 case ID_P:
3091 what = StepDetector;
3092 sname = "StepDetector";
3093 LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
3094 sname.string(), handle,
3095 (mDmpPedometerEnabled? "en": "dis"),
3096 (en? "en" : "dis"));
3097 enableDmpPedometer(en, 1);
3098 mDmpPedometerEnabled = !!en;
3099 batchMode = computeBatchSensorMask(mEnabled, mBatchEnabled);
3100 /* skip setBatch if there is no need to */
3101 if(((int)mOldBatchEnabledMask != batchMode) || batchMode) {
3102 setBatch(batchMode,1);
3103 }
3104 mOldBatchEnabledMask = batchMode;
3105 if (en)
3106 mEnabledTime[StepDetector] = android::elapsedRealtimeNano();
3107 else
3108 mEnabledTime[StepDetector] = 0;
3109
3110 if (!en)
3111 mBatchDelays[what] = 1000000000LL;
3112 return 0;
3113 case ID_SM:
3114 sname = "Significant Motion";
3115 LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
3116 sname.string(), handle,
3117 (mDmpSignificantMotionEnabled? "en": "dis"),
3118 (en? "en" : "dis"));
3119 enableDmpSignificantMotion(en);
3120 mDmpSignificantMotionEnabled = !!en;
3121 if (en)
3122 mEnabledTime[SignificantMotion] = android::elapsedRealtimeNano();
3123 else
3124 mEnabledTime[SignificantMotion] = 0;
3125 return 0;
3126 case ID_SO:
3127 sname = "Screen Orientation";
3128 LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
3129 sname.string(), handle,
3130 (mDmpOrientationEnabled? "en": "dis"),
3131 (en? "en" : "dis"));
3132 enableDmpOrientation(en && isDmpDisplayOrientationOn());
3133 mDmpOrientationEnabled = !!en;
3134 return 0;
3135 case ID_A:
3136 what = Accelerometer;
3137 sname = "Accelerometer";
3138 break;
3139 case ID_M:
3140 what = MagneticField;
3141 sname = "MagneticField";
3142 break;
3143 case ID_RM:
3144 what = RawMagneticField;
3145 sname = "MagneticField Uncalibrated";
3146 break;
3147 case ID_O:
3148 what = Orientation;
3149 sname = "Orientation";
3150 break;
3151 case ID_GY:
3152 what = Gyro;
3153 sname = "Gyro";
3154 break;
3155 case ID_RG:
3156 what = RawGyro;
3157 sname = "Gyro Uncalibrated";
3158 break;
3159 case ID_GR:
3160 what = Gravity;
3161 sname = "Gravity";
3162 break;
3163 case ID_RV:
3164 what = RotationVector;
3165 sname = "RotationVector";
3166 break;
3167 case ID_GRV:
3168 what = GameRotationVector;
3169 sname = "GameRotationVector";
3170 break;
3171 case ID_LA:
3172 what = LinearAccel;
3173 sname = "LinearAccel";
3174 break;
3175 case ID_GMRV:
3176 what = GeomagneticRotationVector;
3177 sname = "GeomagneticRotationVector";
3178 break;
3179 #ifdef ENABLE_PRESSURE
3180 case ID_PS:
3181 what = Pressure;
3182 sname = "Pressure";
3183 break;
3184 #endif
3185 default:
3186 what = handle;
3187 sname = "Others";
3188 break;
3189 }
3190
3191 int newState = en ? 1 : 0;
3192 unsigned long sen_mask;
3193
3194 LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s",
3195 sname.string(), handle,
3196 ((mEnabled & (1 << what)) ? "en" : "dis"),
3197 ((uint32_t(newState) << what) ? "en" : "dis"));
3198 LOGV_IF(ENG_VERBOSE,
3199 "HAL:%s sensor state change what=%d", sname.string(), what);
3200
3201 // pthread_mutex_lock(&mMplMutex);
3202 // pthread_mutex_lock(&mHALMutex);
3203
3204 if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
3205 uint32_t sensor_type;
3206 short flags = newState;
3207 uint32_t lastEnabled = mEnabled, changed = 0;
3208
3209 mEnabled &= ~(1 << what);
3210 mEnabled |= (uint32_t(flags) << what);
3211 if (lastEnabled > mEnabled) {
3212 mEnabledCached = mEnabled;
3213 } else {
3214 mEnabledCached = lastEnabled;
3215 }
3216
3217 LOGV_IF(ENG_VERBOSE, "HAL:flags = %d", flags);
3218 computeLocalSensorMask(mEnabled);
3219 LOGV_IF(ENG_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
3220 LOGV_IF(ENG_VERBOSE, "HAL:last enable : lastEnabled = %d", lastEnabled);
3221 sen_mask = mLocalSensorMask & mMasterSensorMask;
3222 mSensorMask = sen_mask;
3223 LOGV_IF(ENG_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);
3224
3225 switch (what) {
3226 case Gyro:
3227 case RawGyro:
3228 case Accelerometer:
3229 if ((!(mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK) &&
3230 !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK)) &&
3231 ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
3232 changed |= (1 << what);
3233 }
3234 if (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION) {
3235 changed |= (1 << what);
3236 }
3237 break;
3238 case MagneticField:
3239 case RawMagneticField:
3240 if (!(mEnabled & VIRTUAL_SENSOR_9AXES_MASK) &&
3241 ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
3242 changed |= (1 << what);
3243 }
3244 break;
3245 #ifdef ENABLE_PRESSURE
3246 case Pressure:
3247 if ((lastEnabled & (1 << what)) != (mEnabled & (1 << what))) {
3248 changed |= (1 << what);
3249 }
3250 break;
3251 #endif
3252 case GameRotationVector:
3253 if (!en)
3254 storeCalibration();
3255 if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK))
3256 ||
3257 (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
3258 ||
3259 (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK))
3260 ||
3261 (!en && (mEnabled & VIRTUAL_SENSOR_MAG_6AXES_MASK))) {
3262 for (int i = Gyro; i <= RawMagneticField; i++) {
3263 if (!(mEnabled & (1 << i))) {
3264 changed |= (1 << i);
3265 }
3266 }
3267 }
3268 break;
3269
3270 case Orientation:
3271 case RotationVector:
3272 case LinearAccel:
3273 case Gravity:
3274 if (!en)
3275 storeCalibration();
3276 if ((en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
3277 ||
3278 (!en && !(mEnabled & VIRTUAL_SENSOR_9AXES_MASK))) {
3279 for (int i = Gyro; i <= RawMagneticField; i++) {
3280 if (!(mEnabled & (1 << i))) {
3281 changed |= (1 << i);
3282 }
3283 }
3284 }
3285 break;
3286 case GeomagneticRotationVector:
3287 if (!en)
3288 storeCalibration();
3289 if ((en && !(lastEnabled & VIRTUAL_SENSOR_ALL_MASK))
3290 ||
3291 (en && !(lastEnabled & VIRTUAL_SENSOR_9AXES_MASK))
3292 ||
3293 (!en && !(mEnabled & VIRTUAL_SENSOR_ALL_MASK))
3294 ||
3295 (!en && (mEnabled & VIRTUAL_SENSOR_GYRO_6AXES_MASK))) {
3296 for (int i = Accelerometer; i <= RawMagneticField; i++) {
3297 if (!(mEnabled & (1<<i))) {
3298 changed |= (1 << i);
3299 }
3300 }
3301 }
3302 break;
3303 }
3304 LOGV_IF(ENG_VERBOSE, "HAL:changed = %d", changed);
3305 enableSensors(sen_mask, flags, changed);
3306 // update mEnabledCached afer all configurations done
3307 mEnabledCached = mEnabled;
3308
3309 if (en)
3310 mEnabledTime[what] = android::elapsedRealtimeNano();
3311 else
3312 mEnabledTime[what] = 0;
3313
3314 if (!en)
3315 mBatchDelays[what] = 1000000000LL;
3316 }
3317
3318 // pthread_mutex_unlock(&mMplMutex);
3319 // pthread_mutex_unlock(&mHALMutex);
3320
3321 #ifdef INV_PLAYBACK_DBG
3322 /* apparently the logging needs to go through this sequence
3323 to properly flush the log file */
3324 inv_turn_off_data_logging();
3325 if (fclose(logfile) < 0) {
3326 LOGE("cannot close debug log file");
3327 }
3328 logfile = fopen("/data/playback.bin", "ab");
3329 if (logfile)
3330 inv_turn_on_data_logging(logfile);
3331 #endif
3332
3333 return err;
3334 }
3335
3336 void MPLSensor::getHandle(int32_t handle, int &what, android::String8 &sname)
3337 {
3338 VFUNC_LOG;
3339
3340 what = -1;
3341
3342 switch (handle) {
3343 case ID_P:
3344 what = StepDetector;
3345 sname = "StepDetector";
3346 break;
3347 case ID_SC:
3348 what = StepCounter;
3349 sname = "StepCounter";
3350 break;
3351 case ID_SM:
3352 what = SignificantMotion;
3353 sname = "SignificantMotion";
3354 break;
3355 case ID_SO:
3356 what = handle;
3357 sname = "ScreenOrienation";
3358 break;
3359 case ID_A:
3360 what = Accelerometer;
3361 sname = "Accelerometer";
3362 break;
3363 case ID_M:
3364 what = MagneticField;
3365 sname = "MagneticField";
3366 break;
3367 case ID_RM:
3368 what = RawMagneticField;
3369 sname = "MagneticField Uncalibrated";
3370 break;
3371 case ID_O:
3372 what = Orientation;
3373 sname = "Orientation";
3374 break;
3375 case ID_GY:
3376 what = Gyro;
3377 sname = "Gyro";
3378 break;
3379 case ID_RG:
3380 what = RawGyro;
3381 sname = "Gyro Uncalibrated";
3382 break;
3383 case ID_GR:
3384 what = Gravity;
3385 sname = "Gravity";
3386 break;
3387 case ID_RV:
3388 what = RotationVector;
3389 sname = "RotationVector";
3390 break;
3391 case ID_GRV:
3392 what = GameRotationVector;
3393 sname = "GameRotationVector";
3394 break;
3395 case ID_LA:
3396 what = LinearAccel;
3397 sname = "LinearAccel";
3398 break;
3399 #ifdef ENABLE_PRESSURE
3400 case ID_PS:
3401 what = Pressure;
3402 sname = "Pressure";
3403 break;
3404 #endif
3405 default: // this takes care of all the gestures
3406 what = handle;
3407 sname = "Others";
3408 break;
3409 }
3410
3411 LOGI_IF(EXTRA_VERBOSE, "HAL:getHandle - what=%d, sname=%s", what, sname.string());
3412 return;
3413 }
3414
3415 int MPLSensor::setDelay(int32_t handle, int64_t ns)
3416 {
3417 VFUNC_LOG;
3418
3419 android::String8 sname;
3420 int what = -1;
3421
3422 #if 0
3423 // skip the 1st call for enalbing sensors called by ICS/JB sensor service
3424 static int counter_delay = 0;
3425 if (!(mEnabled & (1 << what))) {
3426 counter_delay = 0;
3427 } else {
3428 if (++counter_delay == 1) {
3429 return 0;
3430 }
3431 else {
3432 counter_delay = 0;
3433 }
3434 }
3435 #endif
3436
3437 getHandle(handle, what, sname);
3438 if (uint32_t(what) >= NumSensors)
3439 return -EINVAL;
3440
3441 if (ns < 0)
3442 return -EINVAL;
3443
3444 LOGV_IF(PROCESS_VERBOSE,
3445 "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
3446
3447 // limit all rates to reasonable ones */
3448 if (ns < 5000000LL) {
3449 ns = 5000000LL;
3450 }
3451
3452 /* store request rate to mDelays arrary for each sensor */
3453 int64_t previousDelay = mDelays[what];
3454 mDelays[what] = ns;
3455 LOGV_IF(ENG_VERBOSE, "storing mDelays[%d] = %lld, previousDelay = %lld", what, ns, previousDelay);
3456
3457 switch (what) {
3458 case StepCounter:
3459 /* set limits of delivery rate of events */
3460 mStepCountPollTime = ns;
3461 LOGV_IF(ENG_VERBOSE, "step count rate =%lld ns", ns);
3462 break;
3463 case StepDetector:
3464 case SignificantMotion:
3465 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
3466 case ID_SO:
3467 #endif
3468 LOGV_IF(ENG_VERBOSE, "Step Detect, SMD, SO rate=%lld ns", ns);
3469 break;
3470 case Gyro:
3471 case RawGyro:
3472 case Accelerometer:
3473 // need to update delay since they are different
3474 // resetDataRates was called earlier
3475 // LOGV("what=%d mEnabled=%d mDelays[%d]=%lld previousDelay=%lld",
3476 // what, mEnabled, what, mDelays[what], previousDelay);
3477 if ((mEnabled & (1 << what)) && (previousDelay != mDelays[what])) {
3478 LOGV_IF(ENG_VERBOSE,
3479 "HAL:need to update delay due to resetDataRates");
3480 break;
3481 }
3482 for (int i = Gyro;
3483 i <= Accelerometer + mCompassSensor->isIntegrated();
3484 i++) {
3485 if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
3486 LOGV_IF(ENG_VERBOSE,
3487 "HAL:ignore delay set due to sensor %d", i);
3488 return 0;
3489 }
3490 }
3491 break;
3492
3493 case MagneticField:
3494 case RawMagneticField:
3495 // need to update delay since they are different
3496 // resetDataRates was called earlier
3497 if ((mEnabled & (1 << what)) && (previousDelay != mDelays[what])) {
3498 LOGV_IF(ENG_VERBOSE,
3499 "HAL:need to update delay due to resetDataRates");
3500 break;
3501 }
3502 if (mCompassSensor->isIntegrated() &&
3503 (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
3504 ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
3505 ((mEnabled & (1 << Accelerometer)) &&
3506 ns > mDelays[Accelerometer])) &&
3507 !checkBatchEnabled()) {
3508 /* if request is slower rate, ignore request */
3509 LOGV_IF(ENG_VERBOSE,
3510 "HAL:ignore delay set due to gyro/accel");
3511 return 0;
3512 }
3513 break;
3514
3515 case Orientation:
3516 case RotationVector:
3517 case GameRotationVector:
3518 case GeomagneticRotationVector:
3519 case LinearAccel:
3520 case Gravity:
3521 if (isLowPowerQuatEnabled()) {
3522 LOGV_IF(ENG_VERBOSE,
3523 "HAL:need to update delay due to LPQ");
3524 break;
3525 }
3526
3527 for (int i = 0; i < NumSensors; i++) {
3528 if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
3529 LOGV_IF(ENG_VERBOSE,
3530 "HAL:ignore delay set due to sensor %d", i);
3531 return 0;
3532 }
3533 }
3534 break;
3535 }
3536
3537 // pthread_mutex_lock(&mHALMutex);
3538 int res = update_delay();
3539 // pthread_mutex_unlock(&mHALMutex);
3540 return res;
3541 }
3542
3543 int MPLSensor::update_delay(void)
3544 {
3545 VFUNC_LOG;
3546
3547 int res = 0;
3548 int64_t got;
3549
3550 if (mEnabled) {
3551 int64_t wanted = 1000000000LL;
3552 int64_t wanted_3rd_party_sensor = 1000000000LL;
3553
3554 // Sequence to change sensor's FIFO rate
3555 // 1. reset master enable
3556 // 2. Update delay
3557 // 3. set master enable
3558
3559 // reset master enable
3560 masterEnable(0);
3561
3562 int64_t gyroRate;
3563 int64_t accelRate;
3564 int64_t compassRate;
3565 #ifdef ENABLE_PRESSURE
3566 int64_t pressureRate;
3567 #endif
3568
3569 int rateInus;
3570 int mplGyroRate;
3571 int mplAccelRate;
3572 int mplCompassRate;
3573 int tempRate = wanted;
3574
3575 /* search the minimum delay requested across all enabled sensors */
3576 for (int i = 0; i < NumSensors; i++) {
3577 if (mEnabled & (1 << i)) {
3578 int64_t ns = mDelays[i];
3579 wanted = wanted < ns ? wanted : ns;
3580 }
3581 }
3582
3583 /* initialize rate for each sensor */
3584 gyroRate = wanted;
3585 accelRate = wanted;
3586 compassRate = wanted;
3587 #ifdef ENABLE_PRESSURE
3588 pressureRate = wanted;
3589 #endif
3590 // same delay for 3rd party Accel or Compass
3591 wanted_3rd_party_sensor = wanted;
3592
3593 int enabled_sensors = mEnabled;
3594 int tempFd = -1;
3595
3596 if(mFeatureActiveMask & INV_DMP_BATCH_MODE) {
3597 // set batch rates
3598 LOGV_IF(ENG_VERBOSE, "HAL: mFeatureActiveMask=%016llx", mFeatureActiveMask);
3599 LOGV("HAL: batch mode is set, set batch data rates");
3600 if (setBatchDataRates() < 0) {
3601 LOGE("HAL:ERR can't set batch data rates");
3602 }
3603 } else {
3604 /* set master sampling frequency */
3605 int64_t tempWanted = wanted;
3606 getDmpRate(&tempWanted);
3607 /* driver only looks at sampling frequency if DMP is off */
3608 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
3609 1000000000.f / tempWanted, mpu.gyro_fifo_rate, getTimestamp());
3610 tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
3611 res = write_attribute_sensor(tempFd, 1000000000.f / tempWanted);
3612 LOGE_IF(res < 0, "HAL:sampling frequency update delay error");
3613
3614 if (LA_ENABLED || GR_ENABLED || RV_ENABLED
3615 || GRV_ENABLED || O_ENABLED || GMRV_ENABLED) {
3616 rateInus = (int)wanted / 1000LL;
3617
3618 /* set rate in MPL */
3619 /* compass can only do 100Hz max */
3620 inv_set_gyro_sample_rate(rateInus);
3621 inv_set_accel_sample_rate(rateInus);
3622 inv_set_compass_sample_rate(rateInus);
3623 inv_set_linear_acceleration_sample_rate(rateInus);
3624 inv_set_orientation_sample_rate(rateInus);
3625 inv_set_rotation_vector_sample_rate(rateInus);
3626 inv_set_gravity_sample_rate(rateInus);
3627 inv_set_orientation_geomagnetic_sample_rate(rateInus);
3628 inv_set_rotation_vector_6_axis_sample_rate(rateInus);
3629 inv_set_geomagnetic_rotation_vector_sample_rate(rateInus);
3630
3631 LOGV_IF(ENG_VERBOSE,
3632 "HAL:MPL virtual sensor sample rate: (mpl)=%d us (mpu)=%.2f Hz",
3633 rateInus, 1000000000.f / wanted);
3634 LOGV_IF(ENG_VERBOSE,
3635 "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz",
3636 rateInus, 1000000000.f / gyroRate);
3637 LOGV_IF(ENG_VERBOSE,
3638 "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz",
3639 rateInus, 1000000000.f / accelRate);
3640 LOGV_IF(ENG_VERBOSE,
3641 "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz",
3642 rateInus, 1000000000.f / compassRate);
3643
3644 LOGV_IF(ENG_VERBOSE,
3645 "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3646 if(mFeatureActiveMask & DMP_FEATURE_MASK) {
3647 bool setDMPrate= 0;
3648 gyroRate = wanted;
3649 accelRate = wanted;
3650 compassRate = wanted;
3651 // same delay for 3rd party Accel or Compass
3652 wanted_3rd_party_sensor = wanted;
3653 rateInus = (int)wanted / 1000LL;
3654 // Set LP Quaternion sample rate if enabled
3655 if (checkLPQuaternion()) {
3656 if (wanted <= RATE_200HZ) {
3657 #ifndef USE_LPQ_AT_FASTEST
3658 enableLPQuaternion(0);
3659 #endif
3660 } else {
3661 inv_set_quat_sample_rate(rateInus);
3662 LOGV_IF(ENG_VERBOSE, "HAL:MPL quat sample rate: "
3663 "(mpl)=%d us (mpu)=%.2f Hz",
3664 rateInus, 1000000000.f / wanted);
3665 setDMPrate= 1;
3666 }
3667 }
3668 }
3669
3670 LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
3671 //nsToHz
3672 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
3673 1000000000.f / gyroRate, mpu.gyro_rate,
3674 getTimestamp());
3675 tempFd = open(mpu.gyro_rate, O_RDWR);
3676 res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
3677 if(res < 0) {
3678 LOGE("HAL:GYRO update delay error");
3679 }
3680
3681 if(USE_THIRD_PARTY_ACCEL == 1) {
3682 // 3rd party accelerometer - if applicable
3683 // nsToHz (BMA250)
3684 LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
3685 wanted_3rd_party_sensor / 1000000L, mpu.accel_rate,
3686 getTimestamp());
3687 tempFd = open(mpu.accel_rate, O_RDWR);
3688 res = write_attribute_sensor(tempFd,
3689 wanted_3rd_party_sensor / 1000000L);
3690 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
3691 } else {
3692 // mpu accel
3693 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
3694 1000000000.f / accelRate, mpu.accel_rate,
3695 getTimestamp());
3696 tempFd = open(mpu.accel_rate, O_RDWR);
3697 res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
3698 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
3699 }
3700
3701 if (!mCompassSensor->isIntegrated()) {
3702 // stand-alone compass - if applicable
3703 LOGV_IF(ENG_VERBOSE,
3704 "HAL:Ext compass delay %lld", wanted_3rd_party_sensor);
3705 LOGV_IF(ENG_VERBOSE, "HAL:Ext compass rate %.2f Hz",
3706 1000000000.f / wanted_3rd_party_sensor);
3707 if (wanted_3rd_party_sensor <
3708 mCompassSensor->getMinDelay() * 1000LL) {
3709 wanted_3rd_party_sensor =
3710 mCompassSensor->getMinDelay() * 1000LL;
3711 }
3712 LOGV_IF(ENG_VERBOSE,
3713 "HAL:Ext compass delay %lld", wanted_3rd_party_sensor);
3714 LOGV_IF(ENG_VERBOSE, "HAL:Ext compass rate %.2f Hz",
3715 1000000000.f / wanted_3rd_party_sensor);
3716 mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
3717 got = mCompassSensor->getDelay(ID_M);
3718 inv_set_compass_sample_rate(got / 1000);
3719 } else {
3720 // compass on secondary bus
3721 if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
3722 compassRate = mCompassSensor->getMinDelay() * 1000LL;
3723 }
3724 mCompassSensor->setDelay(ID_M, compassRate);
3725 }
3726 } else {
3727
3728 if (GY_ENABLED || RGY_ENABLED) {
3729 wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
3730 (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
3731 (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
3732 LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3733 inv_set_gyro_sample_rate((int)wanted/1000LL);
3734 LOGV_IF(ENG_VERBOSE,
3735 "HAL:MPL gyro sample rate: (mpl)=%d us", int(wanted/1000LL));
3736 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
3737 1000000000.f / wanted, mpu.gyro_rate, getTimestamp());
3738 tempFd = open(mpu.gyro_rate, O_RDWR);
3739 res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
3740 LOGE_IF(res < 0, "HAL:GYRO update delay error");
3741 }
3742
3743 if (A_ENABLED) { /* there is only 1 fifo rate for MPUxxxx */
3744 if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
3745 wanted = mDelays[Gyro];
3746 } else if (RGY_ENABLED && mDelays[RawGyro]
3747 < mDelays[Accelerometer]) {
3748 wanted = mDelays[RawGyro];
3749 } else {
3750 wanted = mDelays[Accelerometer];
3751 }
3752 LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3753 inv_set_accel_sample_rate((int)wanted/1000LL);
3754 LOGV_IF(ENG_VERBOSE, "HAL:MPL accel sample rate: (mpl)=%d us",
3755 int(wanted/1000LL));
3756 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
3757 1000000000.f / wanted, mpu.accel_rate,
3758 getTimestamp());
3759 tempFd = open(mpu.accel_rate, O_RDWR);
3760 if(USE_THIRD_PARTY_ACCEL == 1) {
3761 //BMA250 in ms
3762 res = write_attribute_sensor(tempFd, wanted / 1000000L);
3763 }
3764 else {
3765 //MPUxxxx in hz
3766 res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
3767 }
3768 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
3769 }
3770
3771 /* Invensense compass calibration */
3772 if (M_ENABLED || RM_ENABLED) {
3773 int64_t compassWanted = (mDelays[MagneticField] <= mDelays[RawMagneticField]?
3774 (mEnabled & (1 << MagneticField)? mDelays[MagneticField]: mDelays[RawMagneticField]):
3775 (mEnabled & (1 << RawMagneticField)? mDelays[RawMagneticField]: mDelays[MagneticField]));
3776 if (!mCompassSensor->isIntegrated()) {
3777 wanted = compassWanted;
3778 } else {
3779 if (GY_ENABLED
3780 && (mDelays[Gyro] < compassWanted)) {
3781 wanted = mDelays[Gyro];
3782 } else if (RGY_ENABLED
3783 && mDelays[RawGyro] < compassWanted) {
3784 wanted = mDelays[RawGyro];
3785 } else if (A_ENABLED && mDelays[Accelerometer]
3786 < compassWanted) {
3787 wanted = mDelays[Accelerometer];
3788 } else {
3789 wanted = compassWanted;
3790 }
3791 LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3792 }
3793
3794 mCompassSensor->setDelay(ID_M, wanted);
3795 got = mCompassSensor->getDelay(ID_M);
3796 inv_set_compass_sample_rate(got / 1000);
3797 LOGV_IF(ENG_VERBOSE, "HAL:MPL compass sample rate: (mpl)=%d us",
3798 int(got/1000LL));
3799 }
3800 #ifdef ENABLE_PRESSURE
3801 if (PS_ENABLED) {
3802 int64_t pressureRate = mDelays[Pressure];
3803 LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3804 mPressureSensor->setDelay(ID_PS, pressureRate);
3805 LOGE_IF(res < 0, "HAL:PRESSURE update delay error");
3806 }
3807 #endif
3808 }
3809
3810 } //end of non batch mode
3811
3812 unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
3813 if (sensors &
3814 (INV_THREE_AXIS_GYRO
3815 | INV_THREE_AXIS_ACCEL
3816 #ifdef ENABLE_PRESSURE
3817 | (INV_ONE_AXIS_PRESSURE * mPressureSensor->isIntegrated())
3818 #endif
3819 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
3820 LOGV_IF(ENG_VERBOSE, "sensors=%lu", sensors);
3821 res = masterEnable(1);
3822 if(res < 0)
3823 return res;
3824 } else { // all sensors idle -> reduce power, unless DMP is needed
3825 LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
3826 if(mFeatureActiveMask & DMP_FEATURE_MASK) {
3827 res = masterEnable(1);
3828 if(res < 0)
3829 return res;
3830 }
3831 }
3832 }
3833
3834 return res;
3835 }
3836
3837 /**
3838 * Should be called after reading at least one of gyro
3839 * compass or accel data. (Also okay for handling all of them).
3840 * @returns 0, if successful, error number if not.
3841 */
3842 int MPLSensor::readEvents(sensors_event_t* data, int count)
3843 {
3844 VHANDLER_LOG;
3845
3846 if (!mSkipExecuteOnData)
3847 inv_execute_on_data();
3848
3849 int numEventReceived = 0;
3850 long msg;
3851
3852 if (count <= 0)
3853 return 0;
3854
3855 msg = inv_get_message_level_0(1);
3856 if (msg) {
3857 if (msg & INV_MSG_MOTION_EVENT) {
3858 LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n");
3859 }
3860 if (msg & INV_MSG_NO_MOTION_EVENT) {
3861 LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n");
3862 /* after the first no motion, the gyro should be
3863 calibrated well */
3864 mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
3865 /* if gyros are on and we got a no motion, set a flag
3866 indicating that the cal file can be written. */
3867 mHaveGoodMpuCal = true;
3868 }
3869 if(msg & INV_MSG_NEW_AB_EVENT) {
3870 LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Accel Bias *****\n");
3871 getAccelBias();
3872 mAccelAccuracy = inv_get_accel_accuracy();
3873 }
3874 if(msg & INV_MSG_NEW_GB_EVENT) {
3875 LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Gyro Bias *****\n");
3876 getGyroBias();
3877 setGyroBias();
3878 }
3879 if(msg & INV_MSG_NEW_FGB_EVENT) {
3880 LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Factory Gyro Bias *****\n");
3881 getFactoryGyroBias();
3882 }
3883 if(msg & INV_MSG_NEW_FAB_EVENT) {
3884 LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Factory Accel Bias *****\n");
3885 getFactoryAccelBias();
3886 }
3887 if(msg & INV_MSG_NEW_CB_EVENT) {
3888 LOGV_IF(EXTRA_VERBOSE, "HAL:***** New Compass Bias *****\n");
3889 getCompassBias();
3890 mCompassAccuracy = inv_get_mag_accuracy();
3891 }
3892 }
3893
3894 if (!mSkipReadEvents) {
3895 for (int i = 0; i < NumSensors; i++) {
3896 int update = 0;
3897
3898 // handle step detector when ped_q is enabled
3899 if(mPedUpdate) {
3900 if (i == StepDetector) {
3901 update = readDmpPedometerEvents(data, count, ID_P, 1);
3902 mPedUpdate = 0;
3903 if(update == 1 && count > 0) {
3904 if (mLastTimestamp[i] != mStepSensorTimestamp) {
3905 count--;
3906 numEventReceived++;
3907 data->timestamp = mStepSensorTimestamp;
3908 data++;
3909 mLastTimestamp[i] = mStepSensorTimestamp;
3910 } else {
3911 ALOGE("Event from type=%d with duplicate timestamp %lld discarded",
3912 mPendingEvents[i].type, mStepSensorTimestamp);
3913 }
3914 continue;
3915 }
3916 } else {
3917 if (mPedUpdate == DATA_FORMAT_STEP) {
3918 continue;
3919 }
3920 }
3921 }
3922
3923 // load up virtual sensors
3924 if (mEnabledCached & (1 << i)) {
3925 update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
3926 mPendingMask |= (1 << i);
3927
3928 if (update && (count > 0)) {
3929 // Discard any events with duplicate timestamps
3930 if (mLastTimestamp[i] != mPendingEvents[i].timestamp) {
3931 mLastTimestamp[i] = mPendingEvents[i].timestamp;
3932 *data++ = mPendingEvents[i];
3933 count--;
3934 numEventReceived++;
3935 } else {
3936 ALOGE("Event from type=%d with duplicate timestamp %lld (%+f, %+f, %+f) discarded",
3937 mPendingEvents[i].type, mLastTimestamp[i], mPendingEvents[i].data[0], mPendingEvents[i].data[1], mPendingEvents[i].data[2]);
3938 }
3939 }
3940 }
3941 mCompassOverFlow = 0;
3942
3943 // handle partial packet read and end marker
3944 // skip readEvents from hal_outputs
3945 if (mFlushBatchSet && count>0 && !mFlushSensorEnabledVector.isEmpty()) {
3946 while (mFlushBatchSet && count>0 && !mFlushSensorEnabledVector.isEmpty()) {
3947 int sendEvent = metaHandler(&mPendingFlushEvents[0], META_DATA_FLUSH_COMPLETE);
3948 if (sendEvent) {
3949 LOGV_IF(ENG_VERBOSE, "Queueing flush complete for handle=%d",
3950 mPendingFlushEvents[0].meta_data.sensor);
3951 *data++ = mPendingFlushEvents[0];
3952 count--;
3953 numEventReceived++;
3954 } else {
3955 LOGV_IF(ENG_VERBOSE, "sendEvent false, NOT queueing flush complete for handle=%d",
3956 mPendingFlushEvents[0].meta_data.sensor);
3957 }
3958 mFlushBatchSet--;
3959 }
3960
3961 // Double check flush status
3962 if (mFlushSensorEnabledVector.isEmpty()) {
3963 mEmptyDataMarkerDetected = 0;
3964 mDataMarkerDetected = 0;
3965 mFlushBatchSet = 0;
3966 LOGV_IF(ENG_VERBOSE, "Flush completed");
3967 } else {
3968 LOGV_IF(ENG_VERBOSE, "Flush is still active");
3969 }
3970 } else if (mFlushBatchSet && mFlushSensorEnabledVector.isEmpty()) {
3971 mFlushBatchSet = 0;
3972 }
3973 }
3974 }
3975 return numEventReceived;
3976 }
3977
3978 // collect data for MPL (but NOT sensor service currently), from driver layer
3979 void MPLSensor::buildMpuEvent(void)
3980 {
3981 VHANDLER_LOG;
3982
3983 mSkipReadEvents = 0;
3984 int64_t mGyroSensorTimestamp=0, mAccelSensorTimestamp=0, latestTimestamp=0;
3985 int lp_quaternion_on = 0, sixAxis_quaternion_on = 0,
3986 ped_quaternion_on = 0, ped_standalone_on = 0;
3987 size_t nbyte;
3988 unsigned short data_format = 0;
3989 int i, nb, mask = 0,
3990 sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
3991 ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
3992 (((mLocalSensorMask & INV_THREE_AXIS_COMPASS)
3993 && mCompassSensor->isIntegrated())? 1 : 0) +
3994 ((mLocalSensorMask & INV_ONE_AXIS_PRESSURE)? 1 : 0);
3995
3996 char *rdata = mIIOBuffer;
3997 ssize_t rsize = 0;
3998 ssize_t readCounter = 0;
3999 char *rdataP = NULL;
4000 bool doneFlag = 0;
4001
4002 /* flush buffer when no sensors are enabled */
4003 if (mEnabledCached == 0 && mBatchEnabled == 0 && mDmpPedometerEnabled == 0) {
4004 rsize = read(iio_fd, rdata, MAX_SUSPEND_BATCH_PACKET_SIZE);
4005 if(rsize > 0) {
4006 LOGV_IF(ENG_VERBOSE, "HAL:input data flush rsize=%d", (int)rsize);
4007 }
4008 mLeftOverBufferSize = 0;
4009 mDataMarkerDetected = 0;
4010 mEmptyDataMarkerDetected = 0;
4011 return;
4012 }
4013
4014 lp_quaternion_on = isLowPowerQuatEnabled() && checkLPQuaternion();
4015 sixAxis_quaternion_on = check6AxisQuatEnabled();
4016 ped_quaternion_on = checkPedQuatEnabled();
4017 ped_standalone_on = checkPedStandaloneEnabled();
4018
4019 nbyte = MAX_READ_SIZE - mLeftOverBufferSize;
4020
4021 /* check previous copied buffer */
4022 /* append with just read data */
4023 if (mLeftOverBufferSize > 0) {
4024 LOGV_IF(0, "append old buffer size=%d", mLeftOverBufferSize);
4025 memset(rdata, 0, sizeof(mIIOBuffer));
4026 memcpy(rdata, mLeftOverBuffer, mLeftOverBufferSize);
4027 LOGV_IF(0,
4028 "HAL:input retrieve old buffer data=:%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, "
4029 "%d, %d,%d, %d, %d, %d\n",
4030 rdata[0], rdata[1], rdata[2], rdata[3],
4031 rdata[4], rdata[5], rdata[6], rdata[7],
4032 rdata[8], rdata[9], rdata[10], rdata[11],
4033 rdata[12], rdata[13], rdata[14], rdata[15]);
4034 }
4035 rdataP = rdata + mLeftOverBufferSize;
4036
4037 /* read expected number of bytes */
4038 rsize = read(iio_fd, rdataP, nbyte);
4039 if(rsize < 0) {
4040 /* IIO buffer might have old data.
4041 Need to flush it if no sensor is on, to avoid infinite
4042 read loop.*/
4043 LOGE("HAL:input data file descriptor not available - (%s)",
4044 strerror(errno));
4045 if (sensors == 0) {
4046 rsize = read(iio_fd, rdata, MAX_SUSPEND_BATCH_PACKET_SIZE);
4047 if(rsize > 0) {
4048 LOGV_IF(ENG_VERBOSE, "HAL:input data flush rsize=%d", (int)rsize);
4049 #ifdef TESTING
4050 LOGV_IF(INPUT_DATA,
4051 "HAL:input rdata:r=%d, n=%d,"
4052 "%d, %d, %d, %d, %d, %d, %d, %d",
4053 (int)rsize, nbyte,
4054 rdataP[0], rdataP[1], rdataP[2], rdataP[3],
4055 rdataP[4], rdataP[5], rdataP[6], rdataP[7]);
4056 #endif
4057 mLeftOverBufferSize = 0;
4058 }
4059 }
4060 return;
4061 }
4062
4063 #ifdef TESTING
4064 LOGV_IF(INPUT_DATA,
4065 "HAL:input just read rdataP:r=%d, n=%d,"
4066 "%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,"
4067 "%d, %d, %d, %d,%d, %d, %d, %d\n",
4068 (int)rsize, nbyte,
4069 rdataP[0], rdataP[1], rdataP[2], rdataP[3],
4070 rdataP[4], rdataP[5], rdataP[6], rdataP[7],
4071 rdataP[8], rdataP[9], rdataP[10], rdataP[11],
4072 rdataP[12], rdataP[13], rdataP[14], rdataP[15],
4073 rdataP[16], rdataP[17], rdataP[18], rdataP[19],
4074 rdataP[20], rdataP[21], rdataP[22], rdataP[23]);
4075
4076 LOGV_IF(INPUT_DATA,
4077 "HAL:input rdata:r=%d, n=%d,"
4078 "%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,%d, %d, %d, %d,"
4079 "%d, %d, %d, %d,%d, %d, %d, %d\n",
4080 (int)rsize, nbyte,
4081 rdata[0], rdata[1], rdata[2], rdata[3],
4082 rdata[4], rdata[5], rdata[6], rdata[7],
4083 rdata[8], rdata[9], rdata[10], rdata[11],
4084 rdata[12], rdata[13], rdata[14], rdata[15],
4085 rdata[16], rdata[17], rdata[18], rdata[19],
4086 rdata[20], rdata[21], rdata[22], rdata[23]);
4087 #endif
4088 /* reset data and count pointer */
4089 rdataP = rdata;
4090 readCounter = rsize + mLeftOverBufferSize;
4091 LOGV_IF(0, "HAL:input readCounter set=%d", (int)readCounter);
4092
4093 if(readCounter < MAX_READ_SIZE) {
4094 // Handle standalone MARKER packet
4095 if (readCounter >= BYTES_PER_SENSOR) {
4096 data_format = *((short *)(rdata));
4097 if (data_format == DATA_FORMAT_MARKER) {
4098 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "MARKER DETECTED:0x%x", data_format);
4099 readCounter -= BYTES_PER_SENSOR;
4100 rdata += BYTES_PER_SENSOR;
4101 if (!mFlushSensorEnabledVector.isEmpty()) {
4102 mFlushBatchSet++;
4103 }
4104 mDataMarkerDetected = 1;
4105 }
4106 else if (data_format == DATA_FORMAT_EMPTY_MARKER) {
4107 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "EMPTY MARKER DETECTED:0x%x", data_format);
4108 readCounter -= BYTES_PER_SENSOR;
4109 rdata += BYTES_PER_SENSOR;
4110 if (!mFlushSensorEnabledVector.isEmpty()) {
4111 mFlushBatchSet++;
4112 }
4113 mEmptyDataMarkerDetected = 1;
4114 mDataMarkerDetected = 1;
4115 }
4116 }
4117
4118 /* store packet then return */
4119 mLeftOverBufferSize = readCounter;
4120 memcpy(mLeftOverBuffer, rdata, mLeftOverBufferSize);
4121
4122 #ifdef TESTING
4123 LOGV_IF(1, "HAL:input data has batched partial packet");
4124 LOGV_IF(1, "HAL:input data batched mLeftOverBufferSize=%d", mLeftOverBufferSize);
4125 LOGV_IF(1,
4126 "HAL:input catch up batched retrieve buffer=:%d, %d, %d, %d, %d, %d, %d, %d,"
4127 "%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
4128 mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3],
4129 mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7],
4130 mLeftOverBuffer[8], mLeftOverBuffer[9], mLeftOverBuffer[10], mLeftOverBuffer[11],
4131 mLeftOverBuffer[12], mLeftOverBuffer[13], mLeftOverBuffer[14], mLeftOverBuffer[15]);
4132 #endif
4133 mSkipReadEvents = 1;
4134 return;
4135 }
4136
4137 LOGV_IF(INPUT_DATA && ENG_VERBOSE,
4138 "HAL:input b=%d rdata= %d nbyte= %d rsize= %d readCounter= %d",
4139 checkBatchEnabled(), *((short *) rdata), nbyte, (int)rsize, (int)readCounter);
4140 LOGV_IF(INPUT_DATA && ENG_VERBOSE,
4141 "HAL:input sensors= %d, lp_q_on= %d, 6axis_q_on= %d, "
4142 "ped_q_on= %d, ped_standalone_on= %d",
4143 sensors, lp_quaternion_on, sixAxis_quaternion_on, ped_quaternion_on,
4144 ped_standalone_on);
4145
4146 mSkipExecuteOnData = 1;
4147 while (readCounter > 0) {
4148 // since copied buffer is already accounted for, reset left over size
4149 mLeftOverBufferSize = 0;
4150 // clear data format mask for parsing the next set of data
4151 mask = 0;
4152 data_format = *((short *)(rdata));
4153 LOGV_IF(INPUT_DATA && ENG_VERBOSE,
4154 "HAL:input data_format=%x", data_format);
4155
4156 if(checkValidHeader(data_format) == 0) {
4157 LOGE("HAL:input invalid data_format 0x%02X", data_format);
4158 return;
4159 }
4160
4161 if (data_format & DATA_FORMAT_STEP) {
4162 if (data_format == DATA_FORMAT_STEP) {
4163 rdata += BYTES_PER_SENSOR;
4164 latestTimestamp = *((long long*) (rdata));
4165 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "STEP DETECTED:0x%x - ts: %lld", data_format, latestTimestamp);
4166 // readCounter is decrement by 24 because DATA_FORMAT_STEP only applies in batch mode
4167 readCounter -= BYTES_PER_SENSOR_PACKET;
4168 } else {
4169 LOGV_IF(0, "STEP DETECTED:0x%x", data_format);
4170 }
4171 mPedUpdate |= data_format;
4172 // cancels step bit
4173 data_format &= (~DATA_FORMAT_STEP);
4174 }
4175
4176 if (data_format == DATA_FORMAT_MARKER) {
4177 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "MARKER DETECTED:0x%x", data_format);
4178 readCounter -= BYTES_PER_SENSOR;
4179 if (!mFlushSensorEnabledVector.isEmpty()) {
4180 mFlushBatchSet++;
4181 }
4182 mDataMarkerDetected = 1;
4183 }
4184 else if (data_format == DATA_FORMAT_EMPTY_MARKER) {
4185 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "EMPTY MARKER DETECTED:0x%x", data_format);
4186 readCounter -= BYTES_PER_SENSOR;
4187 if (!mFlushSensorEnabledVector.isEmpty()) {
4188 mFlushBatchSet++;
4189 }
4190 mEmptyDataMarkerDetected = 1;
4191 mDataMarkerDetected = 1;
4192 }
4193 else if (data_format == DATA_FORMAT_QUAT) {
4194 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "QUAT DETECTED:0x%x", data_format);
4195 if (readCounter >= BYTES_QUAT_DATA) {
4196 mCachedQuaternionData[0] = *((int *) (rdata + 4));
4197 mCachedQuaternionData[1] = *((int *) (rdata + 8));
4198 mCachedQuaternionData[2] = *((int *) (rdata + 12));
4199 rdata += QUAT_ONLY_LAST_PACKET_OFFSET;
4200 mQuatSensorTimestamp = *((long long*) (rdata));
4201 mask |= DATA_FORMAT_QUAT;
4202 readCounter -= BYTES_QUAT_DATA;
4203 }
4204 else {
4205 doneFlag = 1;
4206 }
4207 }
4208 else if (data_format == DATA_FORMAT_6_AXIS) {
4209 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "6AXIS DETECTED:0x%x", data_format);
4210 if (readCounter >= BYTES_QUAT_DATA) {
4211 mCached6AxisQuaternionData[0] = *((int *) (rdata + 4));
4212 mCached6AxisQuaternionData[1] = *((int *) (rdata + 8));
4213 mCached6AxisQuaternionData[2] = *((int *) (rdata + 12));
4214 rdata += QUAT_ONLY_LAST_PACKET_OFFSET;
4215 mQuatSensorTimestamp = *((long long*) (rdata));
4216 mask |= DATA_FORMAT_6_AXIS;
4217 readCounter -= BYTES_QUAT_DATA;
4218 }
4219 else {
4220 doneFlag = 1;
4221 }
4222 }
4223 else if (data_format == DATA_FORMAT_PED_QUAT) {
4224 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "PED QUAT DETECTED:0x%x", data_format);
4225 if (readCounter >= BYTES_PER_SENSOR_PACKET) {
4226 mCachedPedQuaternionData[0] = *((short *) (rdata + 2));
4227 mCachedPedQuaternionData[1] = *((short *) (rdata + 4));
4228 mCachedPedQuaternionData[2] = *((short *) (rdata + 6));
4229 rdata += BYTES_PER_SENSOR;
4230 mQuatSensorTimestamp = *((long long*) (rdata));
4231 mask |= DATA_FORMAT_PED_QUAT;
4232 readCounter -= BYTES_PER_SENSOR_PACKET;
4233 }
4234 else {
4235 doneFlag = 1;
4236 }
4237 }
4238 else if (data_format == DATA_FORMAT_PED_STANDALONE) {
4239 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "STANDALONE STEP DETECTED:0x%x", data_format);
4240 if (readCounter >= BYTES_PER_SENSOR_PACKET) {
4241 rdata += BYTES_PER_SENSOR;
4242 mStepSensorTimestamp = *((long long*) (rdata));
4243 mask |= DATA_FORMAT_PED_STANDALONE;
4244 readCounter -= BYTES_PER_SENSOR_PACKET;
4245 mPedUpdate |= data_format;
4246 }
4247 else {
4248 doneFlag = 1;
4249 }
4250 }
4251 else if (data_format == DATA_FORMAT_GYRO) {
4252 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "GYRO DETECTED:0x%x", data_format);
4253 if (readCounter >= BYTES_PER_SENSOR_PACKET) {
4254 mCachedGyroData[0] = *((short *) (rdata + 2));
4255 mCachedGyroData[1] = *((short *) (rdata + 4));
4256 mCachedGyroData[2] = *((short *) (rdata + 6));
4257 rdata += BYTES_PER_SENSOR;
4258 mGyroSensorTimestamp = *((long long*) (rdata));
4259 mask |= DATA_FORMAT_GYRO;
4260 readCounter -= BYTES_PER_SENSOR_PACKET;
4261 } else {
4262 doneFlag = 1;
4263 }
4264 }
4265 else if (data_format == DATA_FORMAT_ACCEL) {
4266 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "ACCEL DETECTED:0x%x", data_format);
4267 if (readCounter >= BYTES_PER_SENSOR_PACKET) {
4268 mCachedAccelData[0] = *((short *) (rdata + 2));
4269 mCachedAccelData[1] = *((short *) (rdata + 4));
4270 mCachedAccelData[2] = *((short *) (rdata + 6));
4271 rdata += BYTES_PER_SENSOR;
4272 mAccelSensorTimestamp = *((long long*) (rdata));
4273 mask |= DATA_FORMAT_ACCEL;
4274 readCounter -= BYTES_PER_SENSOR_PACKET;
4275 }
4276 else {
4277 doneFlag = 1;
4278 }
4279 }
4280 else if (data_format == DATA_FORMAT_COMPASS) {
4281 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "COMPASS DETECTED:0x%x", data_format);
4282 if (readCounter >= BYTES_PER_SENSOR_PACKET) {
4283 if (mCompassSensor->isIntegrated()) {
4284 mCachedCompassData[0] = *((short *) (rdata + 2));
4285 mCachedCompassData[1] = *((short *) (rdata + 4));
4286 mCachedCompassData[2] = *((short *) (rdata + 6));
4287 rdata += BYTES_PER_SENSOR;
4288 mCompassTimestamp = *((long long*) (rdata));
4289 mask |= DATA_FORMAT_COMPASS;
4290 readCounter -= BYTES_PER_SENSOR_PACKET;
4291 }
4292 }
4293 else {
4294 doneFlag = 1;
4295 }
4296 }
4297 else if (data_format == DATA_FORMAT_COMPASS_OF) {
4298 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "COMPASS OF DETECTED:0x%x", data_format);
4299 mask |= DATA_FORMAT_COMPASS_OF;
4300 mCompassOverFlow = 1;
4301 #ifdef INV_PLAYBACK_DBG
4302 if (readCounter >= BYTES_PER_SENSOR_PACKET) {
4303 if (mCompassSensor->isIntegrated()) {
4304 mCachedCompassData[0] = *((short *) (rdata + 2));
4305 mCachedCompassData[1] = *((short *) (rdata + 4));
4306 mCachedCompassData[2] = *((short *) (rdata + 6));
4307 rdata += BYTES_PER_SENSOR;
4308 mCompassTimestamp = *((long long*) (rdata));
4309 readCounter -= BYTES_PER_SENSOR_PACKET;
4310 }
4311 }
4312 #else
4313 readCounter -= BYTES_PER_SENSOR;
4314 #endif
4315 }
4316 #ifdef ENABLE_PRESSURE
4317 else if (data_format == DATA_FORMAT_PRESSURE) {
4318 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "PRESSURE DETECTED:0x%x", data_format);
4319 if (readCounter >= BYTES_QUAT_DATA) {
4320 if (mPressureSensor->isIntegrated()) {
4321 mCachedPressureData =
4322 ((*((short *)(rdata + 4))) << 16) +
4323 (*((unsigned short *) (rdata + 6)));
4324 rdata += BYTES_PER_SENSOR;
4325 mPressureTimestamp = *((long long*) (rdata));
4326 if (mCachedPressureData != 0) {
4327 mask |= DATA_FORMAT_PRESSURE;
4328 }
4329 readCounter -= BYTES_PER_SENSOR_PACKET;
4330 }
4331 } else{
4332 doneFlag = 1;
4333 }
4334 }
4335 #endif
4336
4337 if(doneFlag == 0) {
4338 rdata += BYTES_PER_SENSOR;
4339 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "HAL: input data doneFlag is zero, readCounter=%d", (int)readCounter);
4340 }
4341 else {
4342 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "HAL: input data doneFlag is set, readCounter=%d", (int)readCounter);
4343 }
4344
4345 /* read ahead and store left over data if any */
4346 if (readCounter != 0) {
4347 int currentBufferCounter = 0;
4348 LOGV_IF(0, "Not enough data readCounter=%d, expected nbyte=%d, rsize=%d", (int)readCounter, nbyte, (int)rsize);
4349 memset(mLeftOverBuffer, 0, sizeof(mLeftOverBuffer));
4350 /* check for end markers, don't save */
4351 data_format = *((short *) (rdata));
4352 if ((data_format == DATA_FORMAT_MARKER) || (data_format == DATA_FORMAT_EMPTY_MARKER)) {
4353 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "s MARKER DETECTED:0x%x", data_format);
4354 rdata += BYTES_PER_SENSOR;
4355 readCounter -= BYTES_PER_SENSOR;
4356 if (!mFlushSensorEnabledVector.isEmpty()) {
4357 mFlushBatchSet++;
4358 }
4359 mDataMarkerDetected = 1;
4360 if (readCounter == 0) {
4361 mLeftOverBufferSize = 0;
4362 if(doneFlag != 0) {
4363 return;
4364 }
4365 }
4366 }
4367 memcpy(mLeftOverBuffer, rdata, readCounter);
4368 LOGV_IF(0,
4369 "HAL:input store rdata=:%d, %d, %d, %d,%d, %d, %d, %d,%d, "
4370 "%d, %d, %d,%d, %d, %d, %d\n",
4371 mLeftOverBuffer[0], mLeftOverBuffer[1], mLeftOverBuffer[2], mLeftOverBuffer[3],
4372 mLeftOverBuffer[4], mLeftOverBuffer[5], mLeftOverBuffer[6], mLeftOverBuffer[7],
4373 mLeftOverBuffer[8], mLeftOverBuffer[9], mLeftOverBuffer[10], mLeftOverBuffer[11],
4374 mLeftOverBuffer[12],mLeftOverBuffer[13],mLeftOverBuffer[14], mLeftOverBuffer[15]);
4375
4376 mLeftOverBufferSize = readCounter;
4377 readCounter = 0;
4378 LOGV_IF(0, "Stored number of bytes:%d", mLeftOverBufferSize);
4379 } else {
4380 /* reset count since this is the last packet for the data set */
4381 readCounter = 0;
4382 mLeftOverBufferSize = 0;
4383 }
4384
4385 /* handle data read */
4386 if (mask == DATA_FORMAT_GYRO) {
4387 /* batch mode does not batch temperature */
4388 /* disable temperature read */
4389 if (!(mFeatureActiveMask & INV_DMP_BATCH_MODE)) {
4390 // send down temperature every 0.5 seconds
4391 // with timestamp measured in "driver" layer
4392 if(mGyroSensorTimestamp - mTempCurrentTime >= 500000000LL) {
4393 mTempCurrentTime = mGyroSensorTimestamp;
4394 long long temperature[2];
4395 if(inv_read_temperature(temperature) == 0) {
4396 LOGV_IF(INPUT_DATA,
4397 "HAL:input inv_read_temperature = %lld, timestamp= %lld",
4398 temperature[0], temperature[1]);
4399 inv_build_temp(temperature[0], temperature[1]);
4400 mSkipExecuteOnData = 0;
4401 }
4402 #ifdef TESTING
4403 long bias[3], temp, temp_slope[3];
4404 inv_get_mpl_gyro_bias(bias, &temp);
4405 inv_get_gyro_ts(temp_slope);
4406 LOGI("T: %.3f "
4407 "GB: %+13f %+13f %+13f "
4408 "TS: %+13f %+13f %+13f "
4409 "\n",
4410 (float)temperature[0] / 65536.f,
4411 (float)bias[0] / 65536.f / 16.384f,
4412 (float)bias[1] / 65536.f / 16.384f,
4413 (float)bias[2] / 65536.f / 16.384f,
4414 temp_slope[0] / 65536.f,
4415 temp_slope[1] / 65536.f,
4416 temp_slope[2] / 65536.f);
4417 #endif
4418 }
4419 }
4420 mPendingMask |= 1 << Gyro;
4421 mPendingMask |= 1 << RawGyro;
4422
4423 inv_build_gyro(mCachedGyroData, mGyroSensorTimestamp);
4424 LOGV_IF(INPUT_DATA,
4425 "HAL:input inv_build_gyro: %+8d %+8d %+8d - %lld",
4426 mCachedGyroData[0], mCachedGyroData[1],
4427 mCachedGyroData[2], mGyroSensorTimestamp);
4428 mSkipExecuteOnData = 0;
4429 latestTimestamp = mGyroSensorTimestamp;
4430 }
4431
4432 if (mask == DATA_FORMAT_ACCEL) {
4433 mPendingMask |= 1 << Accelerometer;
4434 inv_build_accel(mCachedAccelData, 0, mAccelSensorTimestamp);
4435 LOGV_IF(INPUT_DATA,
4436 "HAL:input inv_build_accel: %+8ld %+8ld %+8ld - %lld",
4437 mCachedAccelData[0], mCachedAccelData[1],
4438 mCachedAccelData[2], mAccelSensorTimestamp);
4439 mSkipExecuteOnData = 0;
4440 /* remember inital 6 axis quaternion */
4441 inv_time_t tempTimestamp;
4442 inv_get_6axis_quaternion(mInitial6QuatValue, &tempTimestamp);
4443 if (mInitial6QuatValue[0] != 0 && mInitial6QuatValue[1] != 0 &&
4444 mInitial6QuatValue[2] != 0 && mInitial6QuatValue[3] != 0) {
4445 mInitial6QuatValueAvailable = 1;
4446 LOGV_IF(INPUT_DATA && ENG_VERBOSE,
4447 "HAL:input build 6q init: %+8ld %+8ld %+8ld %+8ld",
4448 mInitial6QuatValue[0], mInitial6QuatValue[1],
4449 mInitial6QuatValue[2], mInitial6QuatValue[3]);
4450 }
4451 latestTimestamp = mAccelSensorTimestamp;
4452 }
4453
4454 if (mask == DATA_FORMAT_COMPASS_OF) {
4455 /* compass overflow detected */
4456 /* reset compass algorithm */
4457 int status = 0;
4458 inv_build_compass(mCachedCompassData, status,
4459 mCompassTimestamp);
4460 LOGV_IF(INPUT_DATA,
4461 "HAL:input inv_build_compass_of: %+8ld %+8ld %+8ld - %lld",
4462 mCachedCompassData[0], mCachedCompassData[1],
4463 mCachedCompassData[2], mCompassTimestamp);
4464 mSkipExecuteOnData = 0;
4465 resetCompass();
4466 }
4467
4468 if ((mask == DATA_FORMAT_COMPASS) && mCompassSensor->isIntegrated()) {
4469 int status = 0;
4470 if (mCompassSensor->providesCalibration()) {
4471 status = mCompassSensor->getAccuracy();
4472 status |= INV_CALIBRATED;
4473 }
4474 inv_build_compass(mCachedCompassData, status,
4475 mCompassTimestamp);
4476 LOGV_IF(INPUT_DATA,
4477 "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
4478 mCachedCompassData[0], mCachedCompassData[1],
4479 mCachedCompassData[2], mCompassTimestamp);
4480 mSkipExecuteOnData = 0;
4481 latestTimestamp = mCompassTimestamp;
4482 }
4483
4484 if (mask == DATA_FORMAT_QUAT) {
4485 /* if bias was applied to DMP bias,
4486 set status bits to disable gyro bias cal */
4487 int status = 0;
4488 if (mGyroBiasApplied == true) {
4489 LOGV_IF(INPUT_DATA && ENG_VERBOSE, "HAL:input dmp bias is used");
4490 status |= INV_BIAS_APPLIED;
4491 }
4492 status |= INV_CALIBRATED | INV_QUAT_3AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */
4493 inv_build_quat(mCachedQuaternionData,
4494 status,
4495 mQuatSensorTimestamp);
4496 LOGV_IF(INPUT_DATA,
4497 "HAL:input inv_build_quat-3x: %+8ld %+8ld %+8ld - %lld",
4498 mCachedQuaternionData[0], mCachedQuaternionData[1],
4499 mCachedQuaternionData[2],
4500 mQuatSensorTimestamp);
4501 mSkipExecuteOnData = 0;
4502 latestTimestamp = mQuatSensorTimestamp;
4503 }
4504
4505 if (mask == DATA_FORMAT_6_AXIS) {
4506 /* if bias was applied to DMP bias,
4507 set status bits to disable gyro bias cal */
4508 int status = 0;
4509 if (mGyroBiasApplied == true) {
4510 LOGV_IF(INPUT_DATA && ENG_VERBOSE, "HAL:input dmp bias is used");
4511 status |= INV_QUAT_6AXIS;
4512 }
4513 status |= INV_CALIBRATED | INV_QUAT_6AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */
4514 inv_build_quat(mCached6AxisQuaternionData,
4515 status,
4516 mQuatSensorTimestamp);
4517 LOGV_IF(INPUT_DATA,
4518 "HAL:input inv_build_quat-6x: %+8ld %+8ld %+8ld - %lld",
4519 mCached6AxisQuaternionData[0], mCached6AxisQuaternionData[1],
4520 mCached6AxisQuaternionData[2], mQuatSensorTimestamp);
4521 mSkipExecuteOnData = 0;
4522 latestTimestamp = mQuatSensorTimestamp;
4523 }
4524
4525 if (mask == DATA_FORMAT_PED_QUAT) {
4526 /* if bias was applied to DMP bias,
4527 set status bits to disable gyro bias cal */
4528 int status = 0;
4529 if (mGyroBiasApplied == true) {
4530 LOGV_IF(INPUT_DATA && ENG_VERBOSE,
4531 "HAL:input dmp bias is used");
4532 status |= INV_QUAT_6AXIS;
4533 }
4534 status |= INV_CALIBRATED | INV_QUAT_6AXIS | INV_QUAT_3ELEMENT; /* default 32 (16/32bits) */
4535 mCachedPedQuaternionData[0] = mCachedPedQuaternionData[0] << 16;
4536 mCachedPedQuaternionData[1] = mCachedPedQuaternionData[1] << 16;
4537 mCachedPedQuaternionData[2] = mCachedPedQuaternionData[2] << 16;
4538 inv_build_quat(mCachedPedQuaternionData,
4539 status,
4540 mQuatSensorTimestamp);
4541 LOGV_IF(INPUT_DATA,
4542 "HAL:HAL:input inv_build_quat-ped_6x: %+8ld %+8ld %+8ld - %lld",
4543 mCachedPedQuaternionData[0], mCachedPedQuaternionData[1],
4544 mCachedPedQuaternionData[2], mQuatSensorTimestamp);
4545 mSkipExecuteOnData = 0;
4546 latestTimestamp = mQuatSensorTimestamp;
4547 }
4548
4549 #ifdef ENABLE_PRESSURE
4550 if ((mask ==DATA_FORMAT_PRESSURE) && mPressureSensor->isIntegrated()) {
4551 int status = 0;
4552 latestTimestamp = mPressureTimestamp;
4553 mPressureUpdate = 1;
4554 inv_build_pressure(mCachedPressureData,
4555 status,
4556 mPressureTimestamp);
4557 LOGV_IF(INPUT_DATA,
4558 "HAL:input inv_build_pressure: %+8ld - %lld",
4559 mCachedPressureData, mPressureTimestamp);
4560 mSkipExecuteOnData = 0;
4561 }
4562 #endif
4563 /* take the latest timestamp */
4564 if (mPedUpdate & DATA_FORMAT_STEP) {
4565 /* work around driver output duplicate step detector bit */
4566 if (latestTimestamp > mStepSensorTimestamp) {
4567 mStepSensorTimestamp = latestTimestamp;
4568 LOGV_IF(INPUT_DATA,
4569 "HAL:input build step: 1 - %lld", mStepSensorTimestamp);
4570 } else {
4571 LOGV_IF(ENG_VERBOSE, "Step data OUT OF ORDER, "
4572 "mPedUpdate = 0x%x last = %lld, ts = %lld",
4573 mPedUpdate, mStepSensorTimestamp, latestTimestamp);
4574 mPedUpdate = 0;
4575 }
4576 }
4577 } //while end
4578 }
4579
4580 int MPLSensor::checkValidHeader(unsigned short data_format)
4581 {
4582 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "check data_format=%x", data_format);
4583
4584 if(data_format == DATA_FORMAT_STEP)
4585 return 1;
4586
4587 if(data_format & DATA_FORMAT_STEP) {
4588 data_format &= (~DATA_FORMAT_STEP);
4589 }
4590
4591 if ((data_format == DATA_FORMAT_PED_STANDALONE) ||
4592 (data_format == DATA_FORMAT_PED_QUAT) ||
4593 (data_format == DATA_FORMAT_6_AXIS) ||
4594 (data_format == DATA_FORMAT_QUAT) ||
4595 (data_format == DATA_FORMAT_COMPASS) ||
4596 (data_format == DATA_FORMAT_GYRO) ||
4597 (data_format == DATA_FORMAT_ACCEL) ||
4598 (data_format == DATA_FORMAT_PRESSURE) ||
4599 (data_format == DATA_FORMAT_EMPTY_MARKER) ||
4600 (data_format == DATA_FORMAT_MARKER) ||
4601 (data_format == DATA_FORMAT_COMPASS_OF))
4602 return 1;
4603 else {
4604 LOGV_IF(ENG_VERBOSE, "bad data_format = %x", data_format);
4605 return 0;
4606 }
4607 }
4608
4609 /* use for both MPUxxxx and third party compass */
4610 void MPLSensor::buildCompassEvent(void)
4611 {
4612 VHANDLER_LOG;
4613
4614 int done = 0;
4615
4616 // pthread_mutex_lock(&mMplMutex);
4617 // pthread_mutex_lock(&mHALMutex);
4618
4619 done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
4620 if(mCompassSensor->isYasCompass()) {
4621 if (mCompassSensor->checkCoilsReset() == 1) {
4622 //Reset relevant compass settings
4623 resetCompass();
4624 }
4625 }
4626 if (done > 0) {
4627 int status = 0;
4628 if (mCompassSensor->providesCalibration()) {
4629 status = mCompassSensor->getAccuracy();
4630 status |= INV_CALIBRATED;
4631 }
4632 inv_build_compass(mCachedCompassData, status,
4633 mCompassTimestamp);
4634 LOGV_IF(INPUT_DATA,
4635 "HAL:input inv_build_compass: %+8ld %+8ld %+8ld - %lld",
4636 mCachedCompassData[0], mCachedCompassData[1],
4637 mCachedCompassData[2], mCompassTimestamp);
4638 mSkipReadEvents = 0;
4639 mSkipExecuteOnData = 0;
4640 }
4641
4642 // pthread_mutex_unlock(&mMplMutex);
4643 // pthread_mutex_unlock(&mHALMutex);
4644 }
4645
4646 int MPLSensor::resetCompass(void)
4647 {
4648 VFUNC_LOG;
4649
4650 //Reset compass cal if enabled
4651 if (mMplFeatureActiveMask & INV_COMPASS_CAL) {
4652 LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
4653 inv_init_vector_compass_cal();
4654 inv_init_magnetic_disturbance();
4655 inv_vector_compass_cal_sensitivity(3);
4656 }
4657
4658 //Reset compass fit if enabled
4659 if (mMplFeatureActiveMask & INV_COMPASS_FIT) {
4660 LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
4661 inv_init_compass_fit();
4662 }
4663
4664 return 0;
4665 }
4666
4667 int MPLSensor::getFd(void) const
4668 {
4669 VFUNC_LOG;
4670 LOGV_IF(EXTRA_VERBOSE, "getFd returning %d", iio_fd);
4671 return iio_fd;
4672 }
4673
4674 int MPLSensor::getAccelFd(void) const
4675 {
4676 VFUNC_LOG;
4677 LOGV_IF(EXTRA_VERBOSE, "getAccelFd returning %d", accel_fd);
4678 return accel_fd;
4679 }
4680
4681 int MPLSensor::getCompassFd(void) const
4682 {
4683 VFUNC_LOG;
4684 int fd = mCompassSensor->getFd();
4685 LOGV_IF(EXTRA_VERBOSE, "getCompassFd returning %d", fd);
4686 return fd;
4687 }
4688
4689 int MPLSensor::turnOffAccelFifo(void)
4690 {
4691 VFUNC_LOG;
4692 int i, res = 0, tempFd;
4693 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4694 0, mpu.accel_fifo_enable, getTimestamp());
4695 res += write_sysfs_int(mpu.accel_fifo_enable, 0);
4696 return res;
4697 }
4698
4699 int MPLSensor::turnOffGyroFifo(void)
4700 {
4701 VFUNC_LOG;
4702 int i, res = 0, tempFd;
4703 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4704 0, mpu.gyro_fifo_enable, getTimestamp());
4705 res += write_sysfs_int(mpu.gyro_fifo_enable, 0);
4706 return res;
4707 }
4708
4709 int MPLSensor::enableDmpOrientation(int en)
4710 {
4711 VFUNC_LOG;
4712 int res = 0;
4713 int enabled_sensors = mEnabled;
4714
4715 if (isMpuNonDmp())
4716 return res;
4717
4718 // reset master enable
4719 res = masterEnable(0);
4720 if (res < 0)
4721 return res;
4722
4723 if (en == 1) {
4724 // Enable DMP orientation
4725 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4726 en, mpu.display_orientation_on, getTimestamp());
4727 if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
4728 LOGE("HAL:ERR can't enable Android orientation");
4729 res = -1; // indicate an err
4730 return res;
4731 }
4732
4733 // enable accel engine
4734 res = enableAccel(1);
4735 if (res < 0)
4736 return res;
4737
4738 // disable accel FIFO
4739 if (!(mLocalSensorMask & mMasterSensorMask & INV_THREE_AXIS_ACCEL)) {
4740 res = turnOffAccelFifo();
4741 if (res < 0)
4742 return res;
4743 }
4744
4745 if (!mEnabled){
4746 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4747 1, mpu.dmp_event_int_on, getTimestamp());
4748 if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
4749 res = -1;
4750 LOGE("HAL:ERR can't enable DMP event interrupt");
4751 }
4752 }
4753
4754 mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION;
4755 LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
4756 } else {
4757 mFeatureActiveMask &= ~INV_DMP_DISPL_ORIENTATION;
4758
4759 if (mFeatureActiveMask == 0) {
4760 // disable accel engine
4761 if (!(mLocalSensorMask & mMasterSensorMask
4762 & INV_THREE_AXIS_ACCEL)) {
4763 res = enableAccel(0);
4764 if (res < 0)
4765 return res;
4766 }
4767 }
4768
4769 if (mEnabled){
4770 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
4771 en, mpu.dmp_event_int_on, getTimestamp());
4772 if (write_sysfs_int(mpu.dmp_event_int_on, en) < 0) {
4773 res = -1;
4774 LOGE("HAL:ERR can't enable DMP event interrupt");
4775 }
4776 }
4777 LOGV_IF(ENG_VERBOSE, "mFeatureActiveMask=%016llx", mFeatureActiveMask);
4778 }
4779
4780 if ((res = computeAndSetDmpState()) < 0)
4781 return res;
4782
4783 if (en || mEnabled || mFeatureActiveMask) {
4784 res = masterEnable(1);
4785 }
4786 return res;
4787 }
4788
4789 int MPLSensor::openDmpOrientFd(void)
4790 {
4791 VFUNC_LOG;
4792
4793 if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
4794 LOGV_IF(PROCESS_VERBOSE,
4795 "HAL:DMP display orientation disabled or file desc opened");
4796 return 0;
4797 }
4798
4799 dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
4800 if (dmp_orient_fd < 0) {
4801 LOGE("HAL:ERR couldn't open dmpOrient node");
4802 return -1;
4803 } else {
4804 LOGV_IF(PROCESS_VERBOSE,
4805 "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
4806 return 0;
4807 }
4808 }
4809
4810 int MPLSensor::closeDmpOrientFd(void)
4811 {
4812 VFUNC_LOG;
4813 if (dmp_orient_fd >= 0)
4814 close(dmp_orient_fd);
4815 return 0;
4816 }
4817
4818 int MPLSensor::dmpOrientHandler(int orient)
4819 {
4820 VFUNC_LOG;
4821 LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
4822 return 0;
4823 }
4824
4825 int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count)
4826 {
4827 VFUNC_LOG;
4828
4829 char dummy[4];
4830 int screen_orientation = 0;
4831 FILE *fp;
4832
4833 fp = fopen(mpu.event_display_orientation, "r");
4834 if (fp == NULL) {
4835 LOGE("HAL:cannot open event_display_orientation");
4836 return 0;
4837 } else {
4838 if (fscanf(fp, "%d\n", &screen_orientation) < 0 || fclose(fp) < 0)
4839 {
4840 LOGE("HAL:cannot write event_display_orientation");
4841 }
4842 }
4843
4844 int numEventReceived = 0;
4845
4846 if (mDmpOrientationEnabled && count > 0) {
4847 sensors_event_t temp;
4848
4849 temp.acceleration.x = 0;
4850 temp.acceleration.y = 0;
4851 temp.acceleration.z = 0;
4852 temp.version = sizeof(sensors_event_t);
4853 temp.sensor = ID_SO;
4854 temp.acceleration.status
4855 = SENSOR_STATUS_UNRELIABLE;
4856 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
4857 temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
4858 temp.screen_orientation = screen_orientation;
4859 #endif
4860 temp.timestamp = android::elapsedRealtimeNano();
4861
4862 *data++ = temp;
4863 count--;
4864 numEventReceived++;
4865 }
4866
4867 // read dummy data per driver's request
4868 dmpOrientHandler(screen_orientation);
4869 read(dmp_orient_fd, dummy, 4);
4870
4871 return numEventReceived;
4872 }
4873
4874 int MPLSensor::getDmpOrientFd(void)
4875 {
4876 VFUNC_LOG;
4877
4878 LOGV_IF(EXTRA_VERBOSE, "getDmpOrientFd returning %d", dmp_orient_fd);
4879 return dmp_orient_fd;
4880
4881 }
4882
4883 int MPLSensor::checkDMPOrientation(void)
4884 {
4885 VFUNC_LOG;
4886 return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
4887 }
4888
4889 int MPLSensor::getDmpRate(int64_t *wanted)
4890 {
4891 VFUNC_LOG;
4892
4893 // set DMP output rate to FIFO
4894 if(mDmpOn == 1) {
4895 setQuaternionRate(*wanted);
4896 if (mFeatureActiveMask & INV_DMP_BATCH_MODE) {
4897 set6AxisQuaternionRate(*wanted);
4898 setPedQuaternionRate(*wanted);
4899 }
4900 // DMP running rate must be @ 200Hz
4901 *wanted= RATE_200HZ;
4902 LOGV_IF(PROCESS_VERBOSE,
4903 "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
4904 }
4905 return 0;
4906 }
4907
4908 int MPLSensor::getPollTime(void)
4909 {
4910 VFUNC_LOG;
4911 return mPollTime;
4912 }
4913
4914 int MPLSensor::getStepCountPollTime(void)
4915 {
4916 VFUNC_LOG;
4917 if (mDmpStepCountEnabled) {
4918 // convert poll time from nS to mS
4919 return (mStepCountPollTime / 1000000LL);
4920 }
4921 return 1000;
4922 }
4923
4924 bool MPLSensor::hasStepCountPendingEvents(void)
4925 {
4926 VFUNC_LOG;
4927 if (mDmpStepCountEnabled) {
4928 int64_t t_now_ns;
4929 int64_t interval = 0;
4930
4931 t_now_ns = android::elapsedRealtimeNano();
4932 interval = t_now_ns - mt_pre_ns;
4933
4934 if (interval < mStepCountPollTime) {
4935 LOGV_IF(0,
4936 "Step Count interval elapsed: %lld, triggered: %lld",
4937 interval, mStepCountPollTime);
4938 return false;
4939 } else {
4940 mt_pre_ns = android::elapsedRealtimeNano();
4941 LOGV_IF(0, "Step Count previous time: %lld ms",
4942 mt_pre_ns / 1000000);
4943 return true;
4944 }
4945 }
4946 return false;
4947 }
4948
4949 bool MPLSensor::hasPendingEvents(void) const
4950 {
4951 VFUNC_LOG;
4952 // if we are using the polling workaround, force the main
4953 // loop to check for data every time
4954 return (mPollTime != -1);
4955 }
4956
4957 int MPLSensor::inv_read_temperature(long long *data)
4958 {
4959 VHANDLER_LOG;
4960
4961 int count = 0;
4962 char raw_buf[40];
4963 long raw = 0;
4964
4965 long long timestamp = 0;
4966
4967 memset(raw_buf, 0, sizeof(raw_buf));
4968 count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
4969 sizeof(raw_buf));
4970 if(count < 1) {
4971 LOGE("HAL:error reading gyro temperature");
4972 return -1;
4973 }
4974
4975 count = sscanf(raw_buf, "%ld%lld", &raw, ×tamp);
4976
4977 if(count < 0) {
4978 return -1;
4979 }
4980
4981 LOGV_IF(ENG_VERBOSE && INPUT_DATA,
4982 "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
4983 raw, timestamp, count);
4984 data[0] = raw;
4985 data[1] = timestamp;
4986
4987 return 0;
4988 }
4989
4990 int MPLSensor::inv_read_dmp_state(int fd)
4991 {
4992 VFUNC_LOG;
4993
4994 if(fd < 0)
4995 return -1;
4996
4997 int count = 0;
4998 char raw_buf[10];
4999 short raw = 0;
5000
5001 memset(raw_buf, 0, sizeof(raw_buf));
5002 count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
5003 if(count < 1) {
5004 LOGE("HAL:error reading dmp state");
5005 close(fd);
5006 return -1;
5007 }
5008 count = sscanf(raw_buf, "%hd", &raw);
5009 if(count < 0) {
5010 LOGE("HAL:dmp state data is invalid");
5011 close(fd);
5012 return -1;
5013 }
5014 LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
5015 close(fd);
5016 return (int)raw;
5017 }
5018
5019 int MPLSensor::inv_read_sensor_bias(int fd, long *data)
5020 {
5021 VFUNC_LOG;
5022
5023 if(fd == -1) {
5024 return -1;
5025 }
5026
5027 char buf[50];
5028 char x[15], y[15], z[15];
5029
5030 memset(buf, 0, sizeof(buf));
5031 int count = read_attribute_sensor(fd, buf, sizeof(buf));
5032 if(count < 1) {
5033 LOGE("HAL:Error reading gyro bias");
5034 return -1;
5035 }
5036 count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
5037 if(count) {
5038 /* scale appropriately for MPL */
5039 LOGV_IF(ENG_VERBOSE,
5040 "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
5041 atol(x), atol(y), atol(z));
5042
5043 data[0] = (long)(atol(x) / 10000 * (1L << 16));
5044 data[1] = (long)(atol(y) / 10000 * (1L << 16));
5045 data[2] = (long)(atol(z) / 10000 * (1L << 16));
5046
5047 LOGV_IF(ENG_VERBOSE,
5048 "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
5049 data[0], data[1], data[2]);
5050 }
5051 return 0;
5052 }
5053
5054 /** fill in the sensor list based on which sensors are configured.
5055 * return the number of configured sensors.
5056 * parameter list must point to a memory region of at least 7*sizeof(sensor_t)
5057 * parameter len gives the length of the buffer pointed to by list
5058 */
5059 int MPLSensor::populateSensorList(struct sensor_t *list, int len)
5060 {
5061 VFUNC_LOG;
5062
5063 int numsensors;
5064
5065 if(len <
5066 (int)((sizeof(sBaseSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
5067 LOGE("HAL:sensor list too small, not populating.");
5068 return -(sizeof(sBaseSensorList) / sizeof(sensor_t));
5069 }
5070
5071 /* fill in the base values */
5072 memcpy(list, sBaseSensorList,
5073 sizeof (struct sensor_t) * (sizeof(sBaseSensorList) / sizeof(sensor_t)));
5074
5075 /* first add gyro, accel and compass to the list */
5076
5077 /* fill in gyro/accel values */
5078 if(chip_ID == NULL) {
5079 LOGE("HAL:Can not get gyro/accel id");
5080 }
5081 fillGyro(chip_ID, list);
5082 fillAccel(chip_ID, list);
5083
5084 // TODO: need fixes for unified HAL and 3rd-party solution
5085 mCompassSensor->fillList(&list[MagneticField]);
5086 mCompassSensor->fillList(&list[RawMagneticField]);
5087 #ifdef ENABLE_PRESSURE
5088 if (mPressureSensor != NULL) {
5089 mPressureSensor->fillList(&list[Pressure]);
5090 }
5091 #endif
5092
5093 if(1) {
5094 numsensors = (sizeof(sBaseSensorList) / sizeof(sensor_t));
5095 /* all sensors will be added to the list
5096 fill in orientation values */
5097 fillOrientation(list);
5098 /* fill in rotation vector values */
5099 fillRV(list);
5100 /* fill in game rotation vector values */
5101 fillGRV(list);
5102 /* fill in gravity values */
5103 fillGravity(list);
5104 /* fill in Linear accel values */
5105 fillLinearAccel(list);
5106 /* fill in Significant motion values */
5107 fillSignificantMotion(list);
5108 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
5109 /* fill in screen orientation values */
5110 fillScreenOrientation(list);
5111 #endif
5112 } else {
5113 /* no 9-axis sensors, zero fill that part of the list */
5114 numsensors = 3;
5115 memset(list + 3, 0, 4 * sizeof(struct sensor_t));
5116 }
5117
5118 return numsensors;
5119 }
5120
5121 void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
5122 {
5123 VFUNC_LOG;
5124
5125 if (accel) {
5126 if(accel != NULL && strcmp(accel, "BMA250") == 0) {
5127 list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
5128 list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
5129 list[Accelerometer].power = ACCEL_BMA250_POWER;
5130 list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
5131 return;
5132 } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
5133 list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
5134 list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
5135 list[Accelerometer].power = ACCEL_MPU6050_POWER;
5136 list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
5137 return;
5138 } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
5139 list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
5140 list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
5141 list[Accelerometer].power = ACCEL_MPU6500_POWER;
5142 list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
5143 return;
5144 } else if (accel != NULL && strcmp(accel, "MPU6515") == 0) {
5145 list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
5146 list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
5147 list[Accelerometer].power = ACCEL_MPU6500_POWER;
5148 list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
5149 return;
5150 } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
5151 list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
5152 list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
5153 list[Accelerometer].power = ACCEL_MPU9150_POWER;
5154 list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
5155 return;
5156 } else if (accel != NULL && strcmp(accel, "MPU9250") == 0) {
5157 list[Accelerometer].maxRange = ACCEL_MPU9250_RANGE;
5158 list[Accelerometer].resolution = ACCEL_MPU9250_RESOLUTION;
5159 list[Accelerometer].power = ACCEL_MPU9250_POWER;
5160 list[Accelerometer].minDelay = ACCEL_MPU9250_MINDELAY;
5161 return;
5162 } else if (accel != NULL && strcmp(accel, "MPU9255") == 0) {
5163 list[Accelerometer].maxRange = ACCEL_MPU9255_RANGE;
5164 list[Accelerometer].resolution = ACCEL_MPU9255_RESOLUTION;
5165 list[Accelerometer].power = ACCEL_MPU9255_POWER;
5166 list[Accelerometer].minDelay = ACCEL_MPU9255_MINDELAY;
5167 return;
5168 } else if (accel != NULL && strcmp(accel, "MPU9350") == 0) {
5169 list[Accelerometer].maxRange = ACCEL_MPU9350_RANGE;
5170 list[Accelerometer].resolution = ACCEL_MPU9350_RESOLUTION;
5171 list[Accelerometer].power = ACCEL_MPU9350_POWER;
5172 list[Accelerometer].minDelay = ACCEL_MPU9350_MINDELAY;
5173 return;
5174 } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
5175 list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
5176 list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
5177 list[Accelerometer].power = ACCEL_BMA250_POWER;
5178 list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
5179 return;
5180 }
5181 }
5182
5183 LOGE("HAL:unknown accel id %s -- "
5184 "params default to mpu6515 and might be wrong.",
5185 accel);
5186 list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
5187 list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
5188 list[Accelerometer].power = ACCEL_MPU6500_POWER;
5189 list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
5190 }
5191
5192 void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
5193 {
5194 VFUNC_LOG;
5195
5196 if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
5197 list[Gyro].maxRange = GYRO_MPU3050_RANGE;
5198 list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
5199 list[Gyro].power = GYRO_MPU3050_POWER;
5200 list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
5201 } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
5202 list[Gyro].maxRange = GYRO_MPU6050_RANGE;
5203 list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
5204 list[Gyro].power = GYRO_MPU6050_POWER;
5205 list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
5206 } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
5207 list[Gyro].maxRange = GYRO_MPU6500_RANGE;
5208 list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
5209 list[Gyro].power = GYRO_MPU6500_POWER;
5210 list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
5211 } else if( gyro != NULL && strcmp(gyro, "MPU6515") == 0) {
5212 list[Gyro].maxRange = GYRO_MPU6500_RANGE;
5213 list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
5214 list[Gyro].power = GYRO_MPU6500_POWER;
5215 list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
5216 } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
5217 list[Gyro].maxRange = GYRO_MPU9150_RANGE;
5218 list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
5219 list[Gyro].power = GYRO_MPU9150_POWER;
5220 list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
5221 } else if( gyro != NULL && strcmp(gyro, "MPU9250") == 0) {
5222 list[Gyro].maxRange = GYRO_MPU9250_RANGE;
5223 list[Gyro].resolution = GYRO_MPU9250_RESOLUTION;
5224 list[Gyro].power = GYRO_MPU9250_POWER;
5225 list[Gyro].minDelay = GYRO_MPU9250_MINDELAY;
5226 } else if( gyro != NULL && strcmp(gyro, "MPU9255") == 0) {
5227 list[Gyro].maxRange = GYRO_MPU9255_RANGE;
5228 list[Gyro].resolution = GYRO_MPU9255_RESOLUTION;
5229 list[Gyro].power = GYRO_MPU9255_POWER;
5230 list[Gyro].minDelay = GYRO_MPU9255_MINDELAY;
5231 } else if( gyro != NULL && strcmp(gyro, "MPU9350") == 0) {
5232 list[Gyro].maxRange = GYRO_MPU9350_RANGE;
5233 list[Gyro].resolution = GYRO_MPU9350_RESOLUTION;
5234 list[Gyro].power = GYRO_MPU9350_POWER;
5235 list[Gyro].minDelay = GYRO_MPU9350_MINDELAY;
5236 } else {
5237 LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
5238 LOGE("HAL:default to use mpu6515 params");
5239 list[Gyro].maxRange = GYRO_MPU6500_RANGE;
5240 list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
5241 list[Gyro].power = GYRO_MPU6500_POWER;
5242 list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
5243 }
5244
5245 list[RawGyro].maxRange = list[Gyro].maxRange;
5246 list[RawGyro].resolution = list[Gyro].resolution;
5247 list[RawGyro].power = list[Gyro].power;
5248 list[RawGyro].minDelay = list[Gyro].minDelay;
5249
5250 return;
5251 }
5252
5253 /* fillRV depends on values of gyro, accel and compass in the list */
5254 void MPLSensor::fillRV(struct sensor_t *list)
5255 {
5256 VFUNC_LOG;
5257
5258 /* compute power on the fly */
5259 list[RotationVector].power = list[Gyro].power +
5260 list[Accelerometer].power +
5261 list[MagneticField].power;
5262 list[RotationVector].resolution = .00001;
5263 list[RotationVector].maxRange = 1.0;
5264 list[RotationVector].minDelay = 5000;
5265
5266 return;
5267 }
5268
5269 /* fillGMRV depends on values of accel and mag in the list */
5270 void MPLSensor::fillGMRV(struct sensor_t *list)
5271 {
5272 VFUNC_LOG;
5273
5274 /* compute power on the fly */
5275 list[GeomagneticRotationVector].power = list[Accelerometer].power +
5276 list[MagneticField].power;
5277 list[GeomagneticRotationVector].resolution = .00001;
5278 list[GeomagneticRotationVector].maxRange = 1.0;
5279 list[GeomagneticRotationVector].minDelay = 5000;
5280
5281 return;
5282 }
5283
5284 /* fillGRV depends on values of gyro and accel in the list */
5285 void MPLSensor::fillGRV(struct sensor_t *list)
5286 {
5287 VFUNC_LOG;
5288
5289 /* compute power on the fly */
5290 list[GameRotationVector].power = list[Gyro].power +
5291 list[Accelerometer].power;
5292 list[GameRotationVector].resolution = .00001;
5293 list[GameRotationVector].maxRange = 1.0;
5294 list[GameRotationVector].minDelay = 5000;
5295
5296 return;
5297 }
5298
5299 void MPLSensor::fillOrientation(struct sensor_t *list)
5300 {
5301 VFUNC_LOG;
5302
5303 list[Orientation].power = list[Gyro].power +
5304 list[Accelerometer].power +
5305 list[MagneticField].power;
5306 list[Orientation].resolution = .00001;
5307 list[Orientation].maxRange = 360.0;
5308 list[Orientation].minDelay = 5000;
5309
5310 return;
5311 }
5312
5313 void MPLSensor::fillGravity( struct sensor_t *list)
5314 {
5315 VFUNC_LOG;
5316
5317 list[Gravity].power = list[Gyro].power +
5318 list[Accelerometer].power +
5319 list[MagneticField].power;
5320 list[Gravity].resolution = .00001;
5321 list[Gravity].maxRange = 9.81;
5322 list[Gravity].minDelay = 5000;
5323
5324 return;
5325 }
5326
5327 void MPLSensor::fillLinearAccel(struct sensor_t *list)
5328 {
5329 VFUNC_LOG;
5330
5331 list[LinearAccel].power = list[Gyro].power +
5332 list[Accelerometer].power +
5333 list[MagneticField].power;
5334 list[LinearAccel].resolution = list[Accelerometer].resolution;
5335 list[LinearAccel].maxRange = list[Accelerometer].maxRange;
5336 list[LinearAccel].minDelay = 5000;
5337
5338 return;
5339 }
5340
5341 void MPLSensor::fillSignificantMotion(struct sensor_t *list)
5342 {
5343 VFUNC_LOG;
5344
5345 list[SignificantMotion].power = list[Accelerometer].power;
5346 list[SignificantMotion].resolution = 1;
5347 list[SignificantMotion].maxRange = 1;
5348 list[SignificantMotion].minDelay = -1;
5349 }
5350
5351 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
5352 void MPLSensor::fillScreenOrientation(struct sensor_t *list)
5353 {
5354 VFUNC_LOG;
5355
5356 list[NumSensors].power = list[Accelerometer].power;
5357 list[NumSensors].resolution = 1;
5358 list[NumSensors].maxRange = 3;
5359 list[NumSensors].minDelay = 0;
5360 }
5361 #endif
5362
5363 int MPLSensor::inv_init_sysfs_attributes(void)
5364 {
5365 VFUNC_LOG;
5366
5367 char sysfs_path[MAX_SYSFS_NAME_LEN];
5368
5369 memset(sysfs_path, 0, sizeof(sysfs_path));
5370
5371 sysfs_names_ptr = (char*)calloc(MAX_SYSFS_ATTRB,
5372 sizeof(char[MAX_SYSFS_NAME_LEN]));
5373 if (sysfs_names_ptr == NULL) {
5374 LOGE("HAL:couldn't alloc mem for sysfs paths");
5375 return -1;
5376 }
5377
5378 char *sptr = sysfs_names_ptr;
5379 char **dptr = reinterpret_cast<char **>(&mpu);
5380 for (size_t i = 0; i < MAX_SYSFS_ATTRB; i++) {
5381 *dptr++ = sptr;
5382 sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
5383 }
5384
5385 // get proper (in absolute) IIO path & build MPU's sysfs paths
5386 inv_get_sysfs_path(sysfs_path);
5387
5388 memcpy(mSysfsPath, sysfs_path, sizeof(sysfs_path));
5389 sprintf(mpu.key, "%s%s", sysfs_path, "/key");
5390 sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
5391 sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
5392 sprintf(mpu.master_enable, "%s%s", sysfs_path, "/master_enable");
5393 sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
5394
5395 sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path,
5396 "/scan_elements/in_timestamp_en");
5397 sprintf(mpu.in_timestamp_index, "%s%s", sysfs_path,
5398 "/scan_elements/in_timestamp_index");
5399 sprintf(mpu.in_timestamp_type, "%s%s", sysfs_path,
5400 "/scan_elements/in_timestamp_type");
5401
5402 sprintf(mpu.dmp_firmware, "%s%s", sysfs_path, "/dmp_firmware");
5403 sprintf(mpu.firmware_loaded, "%s%s", sysfs_path, "/firmware_loaded");
5404 sprintf(mpu.dmp_on, "%s%s", sysfs_path, "/dmp_on");
5405 sprintf(mpu.dmp_int_on, "%s%s", sysfs_path, "/dmp_int_on");
5406 sprintf(mpu.dmp_event_int_on, "%s%s", sysfs_path, "/dmp_event_int_on");
5407 sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
5408
5409 sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");
5410
5411 sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
5412 sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
5413 sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
5414 sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
5415 sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable");
5416 sprintf(mpu.gyro_fsr, "%s%s", sysfs_path, "/in_anglvel_scale");
5417 sprintf(mpu.gyro_fifo_enable, "%s%s", sysfs_path, "/gyro_fifo_enable");
5418 sprintf(mpu.gyro_rate, "%s%s", sysfs_path, "/gyro_rate");
5419
5420 sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accel_enable");
5421 sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
5422 sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accel_matrix");
5423 sprintf(mpu.accel_fifo_enable, "%s%s", sysfs_path, "/accel_fifo_enable");
5424 sprintf(mpu.accel_rate, "%s%s", sysfs_path, "/accel_rate");
5425
5426 #ifndef THIRD_PARTY_ACCEL //MPU3050
5427 sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
5428
5429 // DMP uses these values
5430 sprintf(mpu.in_accel_x_dmp_bias, "%s%s", sysfs_path, "/in_accel_x_dmp_bias");
5431 sprintf(mpu.in_accel_y_dmp_bias, "%s%s", sysfs_path, "/in_accel_y_dmp_bias");
5432 sprintf(mpu.in_accel_z_dmp_bias, "%s%s", sysfs_path, "/in_accel_z_dmp_bias");
5433
5434 // MPU uses these values
5435 sprintf(mpu.in_accel_x_offset, "%s%s", sysfs_path, "/in_accel_x_offset");
5436 sprintf(mpu.in_accel_y_offset, "%s%s", sysfs_path, "/in_accel_y_offset");
5437 sprintf(mpu.in_accel_z_offset, "%s%s", sysfs_path, "/in_accel_z_offset");
5438 sprintf(mpu.in_accel_self_test_scale, "%s%s", sysfs_path, "/in_accel_self_test_scale");
5439 #endif
5440
5441 // DMP uses these bias values
5442 sprintf(mpu.in_gyro_x_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_x_dmp_bias");
5443 sprintf(mpu.in_gyro_y_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_y_dmp_bias");
5444 sprintf(mpu.in_gyro_z_dmp_bias, "%s%s", sysfs_path, "/in_anglvel_z_dmp_bias");
5445
5446 // MPU uses these bias values
5447 sprintf(mpu.in_gyro_x_offset, "%s%s", sysfs_path, "/in_anglvel_x_offset");
5448 sprintf(mpu.in_gyro_y_offset, "%s%s", sysfs_path, "/in_anglvel_y_offset");
5449 sprintf(mpu.in_gyro_z_offset, "%s%s", sysfs_path, "/in_anglvel_z_offset");
5450 sprintf(mpu.in_gyro_self_test_scale, "%s%s", sysfs_path, "/in_anglvel_self_test_scale");
5451
5452 sprintf(mpu.three_axis_q_on, "%s%s", sysfs_path, "/three_axes_q_on"); //formerly quaternion_on
5453 sprintf(mpu.three_axis_q_rate, "%s%s", sysfs_path, "/three_axes_q_rate");
5454
5455 sprintf(mpu.ped_q_on, "%s%s", sysfs_path, "/ped_q_on");
5456 sprintf(mpu.ped_q_rate, "%s%s", sysfs_path, "/ped_q_rate");
5457
5458 sprintf(mpu.six_axis_q_on, "%s%s", sysfs_path, "/six_axes_q_on");
5459 sprintf(mpu.six_axis_q_rate, "%s%s", sysfs_path, "/six_axes_q_rate");
5460
5461 sprintf(mpu.six_axis_q_value, "%s%s", sysfs_path, "/six_axes_q_value");
5462
5463 sprintf(mpu.step_detector_on, "%s%s", sysfs_path, "/step_detector_on");
5464 sprintf(mpu.step_indicator_on, "%s%s", sysfs_path, "/step_indicator_on");
5465
5466 sprintf(mpu.display_orientation_on, "%s%s", sysfs_path,
5467 "/display_orientation_on");
5468 sprintf(mpu.event_display_orientation, "%s%s", sysfs_path,
5469 "/event_display_orientation");
5470
5471 sprintf(mpu.event_smd, "%s%s", sysfs_path,
5472 "/event_smd");
5473 sprintf(mpu.smd_enable, "%s%s", sysfs_path,
5474 "/smd_enable");
5475 sprintf(mpu.smd_delay_threshold, "%s%s", sysfs_path,
5476 "/smd_delay_threshold");
5477 sprintf(mpu.smd_delay_threshold2, "%s%s", sysfs_path,
5478 "/smd_delay_threshold2");
5479 sprintf(mpu.smd_threshold, "%s%s", sysfs_path,
5480 "/smd_threshold");
5481 sprintf(mpu.batchmode_timeout, "%s%s", sysfs_path,
5482 "/batchmode_timeout");
5483 sprintf(mpu.batchmode_wake_fifo_full_on, "%s%s", sysfs_path,
5484 "/batchmode_wake_fifo_full_on");
5485 sprintf(mpu.flush_batch, "%s%s", sysfs_path,
5486 "/flush_batch");
5487 sprintf(mpu.pedometer_on, "%s%s", sysfs_path,
5488 "/pedometer_on");
5489 sprintf(mpu.pedometer_int_on, "%s%s", sysfs_path,
5490 "/pedometer_int_on");
5491 sprintf(mpu.event_pedometer, "%s%s", sysfs_path,
5492 "/event_pedometer");
5493 sprintf(mpu.pedometer_steps, "%s%s", sysfs_path,
5494 "/pedometer_steps");
5495 sprintf(mpu.pedometer_step_thresh, "%s%s", sysfs_path,
5496 "/pedometer_step_thresh");
5497 sprintf(mpu.pedometer_counter, "%s%s", sysfs_path,
5498 "/pedometer_counter");
5499 sprintf(mpu.motion_lpa_on, "%s%s", sysfs_path,
5500 "/motion_lpa_on");
5501 return 0;
5502 }
5503
5504 //DMP support only for MPU6xxx/9xxx
5505 bool MPLSensor::isMpuNonDmp(void)
5506 {
5507 VFUNC_LOG;
5508 if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050"))
5509 return true;
5510 else
5511 return false;
5512 }
5513
5514 int MPLSensor::isLowPowerQuatEnabled(void)
5515 {
5516 VFUNC_LOG;
5517 #ifdef ENABLE_LP_QUAT_FEAT
5518 return !isMpuNonDmp();
5519 #else
5520 return 0;
5521 #endif
5522 }
5523
5524 int MPLSensor::isDmpDisplayOrientationOn(void)
5525 {
5526 VFUNC_LOG;
5527 #ifdef ENABLE_DMP_DISPL_ORIENT_FEAT
5528 if (isMpuNonDmp())
5529 return 0;
5530 return 1;
5531 #else
5532 return 0;
5533 #endif
5534 }
5535
5536 /* these functions can be consolidated
5537 with inv_convert_to_body_with_scale */
5538 void MPLSensor::getCompassBias()
5539 {
5540 VFUNC_LOG;
5541
5542
5543 long bias[3];
5544 long compassBias[3];
5545 unsigned short orient;
5546 signed char orientMtx[9];
5547 mCompassSensor->getOrientationMatrix(orientMtx);
5548 orient = inv_orientation_matrix_to_scalar(orientMtx);
5549
5550 /* Get Values from MPL */
5551 inv_get_compass_bias(bias);
5552 inv_convert_to_body(orient, bias, compassBias);
5553 LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) %ld %ld %ld", bias[0], bias[1], bias[2]);
5554 LOGV_IF(HANDLER_DATA, "Mpl Compass Bias (HW unit) (body) %ld %ld %ld", compassBias[0], compassBias[1], compassBias[2]);
5555 long compassSensitivity = inv_get_compass_sensitivity();
5556 if (compassSensitivity == 0) {
5557 compassSensitivity = mCompassScale;
5558 }
5559 for(int i=0; i<3; i++) {
5560 /* convert to uT */
5561 float temp = (float) compassSensitivity / (1L << 30);
5562 mCompassBias[i] =(float) (compassBias[i] * temp / 65536.f);
5563 }
5564
5565 return;
5566 }
5567
5568 void MPLSensor::getFactoryGyroBias()
5569 {
5570 VFUNC_LOG;
5571
5572 /* Get Values from MPL */
5573 inv_get_gyro_bias(mFactoryGyroBias);
5574 LOGV_IF(ENG_VERBOSE, "Factory Gyro Bias %ld %ld %ld", mFactoryGyroBias[0], mFactoryGyroBias[1], mFactoryGyroBias[2]);
5575 mFactoryGyroBiasAvailable = true;
5576
5577 return;
5578 }
5579
5580 /* set bias from factory cal file to MPU offset (in chip frame)
5581 x = values store in cal file --> (v/1000 * 2^16 / (2000/250))
5582 offset = x/2^16 * (Gyro scale / self test scale used) * (-1) / offset scale
5583 i.e. self test default scale = 250
5584 gyro scale default to = 2000
5585 offset scale = 4 //as spec by hardware
5586 offset = x/2^16 * (8) * (-1) / (4)
5587 */
5588 void MPLSensor::setFactoryGyroBias()
5589 {
5590 VFUNC_LOG;
5591 int scaleRatio = mGyroScale / mGyroSelfTestScale;
5592 int offsetScale = 4;
5593 LOGV_IF(ENG_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio);
5594 LOGV_IF(ENG_VERBOSE, "HAL: offsetScale used =%d", offsetScale);
5595
5596 /* Write to Driver */
5597 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5598 (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale),
5599 mpu.in_gyro_x_offset, getTimestamp());
5600 if(write_attribute_sensor_continuous(gyro_x_offset_fd,
5601 (((int) (((float) mFactoryGyroBias[0]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
5602 {
5603 LOGE("HAL:Error writing to gyro_x_offset");
5604 return;
5605 }
5606 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5607 (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale),
5608 mpu.in_gyro_y_offset, getTimestamp());
5609 if(write_attribute_sensor_continuous(gyro_y_offset_fd,
5610 (((int) (((float) mFactoryGyroBias[1]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
5611 {
5612 LOGE("HAL:Error writing to gyro_y_offset");
5613 return;
5614 }
5615 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
5616 (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale),
5617 mpu.in_gyro_z_offset, getTimestamp());
5618 if(write_attribute_sensor_continuous(gyro_z_offset_fd,
5619 (((int) (((float) mFactoryGyroBias[2]) / 65536.f * scaleRatio)) * -1 / offsetScale)) < 0)
5620 {
5621 LOGE("HAL:Error writing to gyro_z_offset");
5622 return;
5623 }
5624 mFactoryGyroBiasAvailable = false;
5625 LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Gyro Calibrated Bias Applied");
5626
5627 return;
5628 }
5629
5630 /* these functions can be consolidated
5631 with inv_convert_to_body_with_scale */
5632 void MPLSensor::getGyroBias()
5633 {
5634 VFUNC_LOG;
5635
5636 long *temp = NULL;
5637 long chipBias[3];
5638 long bias[3];
5639 unsigned short orient;
5640
5641 /* Get Values from MPL */
5642 inv_get_mpl_gyro_bias(mGyroChipBias, temp);
5643 orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
5644 inv_convert_to_body(orient, mGyroChipBias, bias);
5645 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "Mpl Gyro Bias (HW unit) %ld %ld %ld", mGyroChipBias[0], mGyroChipBias[1], mGyroChipBias[2]);
5646 LOGV_IF(ENG_VERBOSE && INPUT_DATA, "Mpl Gyro Bias (HW unit) (body) %ld %ld %ld", bias[0], bias[1], bias[2]);
5647 long gyroSensitivity = inv_get_gyro_sensitivity();
5648 if(gyroSensitivity == 0) {
5649 gyroSensitivity = mGyroScale;
5650 }
5651
5652 /* scale and convert to rad */
5653 for(int i=0; i<3; i++) {
5654 float temp = (float) gyroSensitivity / (1L << 30);
5655 mGyroBias[i] = (float) (bias[i] * temp / (1<<16) / 180 * M_PI);
5656 if (mGyroBias[i] != 0)
5657 mGyroBiasAvailable = true;
5658 }
5659
5660 return;
5661 }
5662
5663 void MPLSensor::setGyroZeroBias()
5664 {
5665 VFUNC_LOG;
5666
5667 /* Write to Driver */
5668 LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)",
5669 0, mpu.in_gyro_x_dmp_bias, getTimestamp());
5670 if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, 0) < 0) {
5671 LOGE("HAL:Error writing to gyro_x_dmp_bias");
5672 return;
5673 }
5674 LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)",
5675 0, mpu.in_gyro_y_dmp_bias, getTimestamp());
5676 if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, 0) < 0) {
5677 LOGE("HAL:Error writing to gyro_y_dmp_bias");
5678 return;
5679 }
5680 LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %d > %s (%lld)",
5681 0, mpu.in_gyro_z_dmp_bias, getTimestamp());
5682 if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, 0) < 0) {
5683 LOGE("HAL:Error writing to gyro_z_dmp_bias");
5684 return;
5685 }
5686 LOGV_IF(EXTRA_VERBOSE, "HAL:Zero Gyro DMP Calibrated Bias Applied");
5687
5688 return;
5689 }
5690
5691 void MPLSensor::setGyroBias()
5692 {
5693 VFUNC_LOG;
5694
5695 if(mGyroBiasAvailable == false)
5696 return;
5697
5698 long bias[3];
5699 long gyroSensitivity = inv_get_gyro_sensitivity();
5700
5701 if(gyroSensitivity == 0) {
5702 gyroSensitivity = mGyroScale;
5703 }
5704
5705 inv_get_gyro_bias_dmp_units(bias);
5706
5707 /* Write to Driver */
5708 LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
5709 bias[0], mpu.in_gyro_x_dmp_bias, getTimestamp());
5710 if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, bias[0]) < 0) {
5711 LOGE("HAL:Error writing to gyro_x_dmp_bias");
5712 return;
5713 }
5714 LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
5715 bias[1], mpu.in_gyro_y_dmp_bias, getTimestamp());
5716 if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, bias[1]) < 0) {
5717 LOGE("HAL:Error writing to gyro_y_dmp_bias");
5718 return;
5719 }
5720 LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
5721 bias[2], mpu.in_gyro_z_dmp_bias, getTimestamp());
5722 if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, bias[2]) < 0) {
5723 LOGE("HAL:Error writing to gyro_z_dmp_bias");
5724 return;
5725 }
5726 mGyroBiasApplied = true;
5727 mGyroBiasAvailable = false;
5728 LOGV_IF(EXTRA_VERBOSE, "HAL:Gyro DMP Calibrated Bias Applied");
5729
5730 return;
5731 }
5732
5733 void MPLSensor::getFactoryAccelBias()
5734 {
5735 VFUNC_LOG;
5736
5737 long temp;
5738
5739 /* Get Values from MPL */
5740 inv_get_accel_bias(mFactoryAccelBias);
5741 LOGV_IF(ENG_VERBOSE, "Factory Accel Bias (mg) %ld %ld %ld", mFactoryAccelBias[0], mFactoryAccelBias[1], mFactoryAccelBias[2]);
5742 mFactoryAccelBiasAvailable = true;
5743
5744 return;
5745 }
5746
5747 void MPLSensor::setFactoryAccelBias()
5748 {
5749 VFUNC_LOG;
5750
5751 if(mFactoryAccelBiasAvailable == false)
5752 return;
5753
5754 /* add scaling here - depends on self test parameters */
5755 int scaleRatio = mAccelScale / mAccelSelfTestScale;
5756 int offsetScale = 16;
5757 long tempBias;
5758
5759 LOGV_IF(ENG_VERBOSE, "HAL: scaleRatio used =%d", scaleRatio);
5760 LOGV_IF(ENG_VERBOSE, "HAL: offsetScale used =%d", offsetScale);
5761
5762 /* Write to Driver */
5763 tempBias = -mFactoryAccelBias[0] / 65536.f * scaleRatio / offsetScale;
5764 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
5765 tempBias, mpu.in_accel_x_offset, getTimestamp());
5766 if(write_attribute_sensor_continuous(accel_x_offset_fd, tempBias) < 0) {
5767 LOGE("HAL:Error writing to accel_x_offset");
5768 return;
5769 }
5770 tempBias = -mFactoryAccelBias[1] / 65536.f * scaleRatio / offsetScale;
5771 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
5772 tempBias, mpu.in_accel_y_offset, getTimestamp());
5773 if(write_attribute_sensor_continuous(accel_y_offset_fd, tempBias) < 0) {
5774 LOGE("HAL:Error writing to accel_y_offset");
5775 return;
5776 }
5777 tempBias = -mFactoryAccelBias[2] / 65536.f * scaleRatio / offsetScale;
5778 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %ld > %s (%lld)",
5779 tempBias, mpu.in_accel_z_offset, getTimestamp());
5780 if(write_attribute_sensor_continuous(accel_z_offset_fd, tempBias) < 0) {
5781 LOGE("HAL:Error writing to accel_z_offset");
5782 return;
5783 }
5784 mFactoryAccelBiasAvailable = false;
5785 LOGV_IF(EXTRA_VERBOSE, "HAL:Factory Accel Calibrated Bias Applied");
5786
5787 return;
5788 }
5789
5790 void MPLSensor::getAccelBias()
5791 {
5792 VFUNC_LOG;
5793 long temp;
5794
5795 /* Get Values from MPL */
5796 inv_get_mpl_accel_bias(mAccelBias, &temp);
5797 LOGV_IF(ENG_VERBOSE, "Accel Bias (mg) %ld %ld %ld",
5798 mAccelBias[0], mAccelBias[1], mAccelBias[2]);
5799 mAccelBiasAvailable = true;
5800
5801 return;
5802 }
5803
5804 /* set accel bias obtained from MPL
5805 bias is scaled by 65536 from MPL
5806 DMP expects: bias * 536870912 / 2^30 = bias / 2 (in body frame)
5807 */
5808 void MPLSensor::setAccelBias()
5809 {
5810 VFUNC_LOG;
5811
5812 if(mAccelBiasAvailable == false) {
5813 LOGV_IF(ENG_VERBOSE, "HAL: setAccelBias - accel bias not available");
5814 return;
5815 }
5816
5817 /* write to driver */
5818 LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
5819 (long) (mAccelBias[0] / 65536.f / 2),
5820 mpu.in_accel_x_dmp_bias, getTimestamp());
5821 if(write_attribute_sensor_continuous(
5822 accel_x_dmp_bias_fd, (long)(mAccelBias[0] / 65536.f / 2)) < 0) {
5823 LOGE("HAL:Error writing to accel_x_dmp_bias");
5824 return;
5825 }
5826 LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
5827 (long)(mAccelBias[1] / 65536.f / 2),
5828 mpu.in_accel_y_dmp_bias, getTimestamp());
5829 if(write_attribute_sensor_continuous(
5830 accel_y_dmp_bias_fd, (long)(mAccelBias[1] / 65536.f / 2)) < 0) {
5831 LOGE("HAL:Error writing to accel_y_dmp_bias");
5832 return;
5833 }
5834 LOGV_IF(SYSFS_VERBOSE && INPUT_DATA, "HAL:sysfs:echo %ld > %s (%lld)",
5835 (long)(mAccelBias[2] / 65536 / 2),
5836 mpu.in_accel_z_dmp_bias, getTimestamp());
5837 if(write_attribute_sensor_continuous(
5838 accel_z_dmp_bias_fd, (long)(mAccelBias[2] / 65536 / 2)) < 0) {
5839 LOGE("HAL:Error writing to accel_z_dmp_bias");
5840 return;
5841 }
5842
5843 mAccelBiasAvailable = false;
5844 mAccelBiasApplied = true;
5845 LOGV_IF(EXTRA_VERBOSE, "HAL:Accel DMP Calibrated Bias Applied");
5846
5847 return;
5848 }
5849
5850 int MPLSensor::isCompassDisabled(void)
5851 {
5852 VFUNC_LOG;
5853 if(mCompassSensor->getFd() < 0 && !mCompassSensor->isIntegrated()) {
5854 LOGI_IF(EXTRA_VERBOSE, "HAL: Compass is disabled, Six-axis Sensor Fusion is used.");
5855 return 1;
5856 }
5857 return 0;
5858 }
5859
5860 int MPLSensor::checkBatchEnabled(void)
5861 {
5862 VFUNC_LOG;
5863 return ((mFeatureActiveMask & INV_DMP_BATCH_MODE)? 1:0);
5864 }
5865
5866 /* precondition: framework disallows this case, ie enable continuous sensor, */
5867 /* and enable batch sensor */
5868 /* if one sensor is in continuous mode, HAL disallows enabling batch for this sensor */
5869 /* or any other sensors */
5870 int MPLSensor::batch(int handle, int flags, int64_t period_ns, int64_t timeout)
5871 {
5872 VFUNC_LOG;
5873
5874 int res = 0;
5875
5876 if (isMpuNonDmp())
5877 return res;
5878
5879 /* Enables batch mode and sets timeout for the given sensor */
5880 /* enum SENSORS_BATCH_DRY_RUN, SENSORS_BATCH_WAKE_UPON_FIFO_FULL */
5881 bool dryRun = false;
5882 android::String8 sname;
5883 int what = -1;
5884 int enabled_sensors = mEnabled;
5885 int batchMode = timeout > 0 ? 1 : 0;
5886
5887 LOGI_IF(DEBUG_BATCHING || ENG_VERBOSE,
5888 "HAL:batch called - handle=%d, flags=%d, period=%lld, timeout=%lld",
5889 handle, flags, period_ns, timeout);
5890
5891 if(flags & SENSORS_BATCH_DRY_RUN) {
5892 dryRun = true;
5893 LOGI_IF(PROCESS_VERBOSE,
5894 "HAL:batch - dry run mode is set (%d)", SENSORS_BATCH_DRY_RUN);
5895 }
5896
5897 /* check if we can support issuing interrupt before FIFO fills-up */
5898 /* in a given timeout. */
5899 if (flags & SENSORS_BATCH_WAKE_UPON_FIFO_FULL) {
5900 LOGE("HAL: batch SENSORS_BATCH_WAKE_UPON_FIFO_FULL is not supported");
5901 return -EINVAL;
5902 }
5903
5904 getHandle(handle, what, sname);
5905 if(uint32_t(what) >= NumSensors) {
5906 LOGE("HAL:batch sensors %d not found", what);
5907 return -EINVAL;
5908 }
5909
5910 LOGV_IF(PROCESS_VERBOSE,
5911 "HAL:batch : %llu ns, (%.2f Hz)", period_ns, 1000000000.f / period_ns);
5912
5913 // limit all rates to reasonable ones */
5914 if (period_ns < 5000000LL) {
5915 period_ns = 5000000LL;
5916 } else if (period_ns > 200000000LL) {
5917 period_ns = 200000000LL;
5918 }
5919
5920 LOGV_IF(PROCESS_VERBOSE,
5921 "HAL:batch after applying upper and lower limit: %llu ns, (%.2f Hz)",
5922 period_ns, 1000000000.f / period_ns);
5923
5924 LOGV_IF(PROCESS_VERBOSE,
5925 "HAL:batch after applying upper and lower limit: %llu ns, (%.2f Hz)",
5926 period_ns, 1000000000.f / period_ns);
5927
5928 switch (what) {
5929 case Gyro:
5930 case RawGyro:
5931 case Accelerometer:
5932 #ifdef ENABLE_PRESSURE
5933 case Pressure:
5934 #endif
5935 case GameRotationVector:
5936 case StepDetector:
5937 LOGV_IF(PROCESS_VERBOSE, "HAL: batch - select sensor (handle %d)", handle);
5938 break;
5939 case MagneticField:
5940 case RawMagneticField:
5941 if(timeout > 0 && !mCompassSensor->isIntegrated())
5942 return -EINVAL;
5943 else
5944 LOGV_IF(PROCESS_VERBOSE, "HAL: batch - select sensor (handle %d)", handle);
5945 break;
5946 default:
5947 if (timeout > 0) {
5948 LOGE("sensor (handle %d) is not supported in batch mode", handle);
5949 return -EINVAL;
5950 }
5951 }
5952
5953 if(dryRun == true) {
5954 LOGI("HAL: batch Dry Run is complete");
5955 return 0;
5956 }
5957
5958 if (what == StepCounter) {
5959 mStepCountPollTime = period_ns;
5960 LOGI("HAL: set step count poll time = %lld nS (%.2f Hz)",
5961 mStepCountPollTime, 1000000000.f / mStepCountPollTime);
5962 }
5963
5964 int tempBatch = 0;
5965 if (timeout > 0) {
5966 tempBatch = mBatchEnabled | (1 << what);
5967 } else {
5968 tempBatch = mBatchEnabled & ~(1 << what);
5969 }
5970
5971 if (!computeBatchSensorMask(mEnabled, tempBatch)) {
5972 batchMode = 0;
5973 } else {
5974 batchMode = 1;
5975 }
5976
5977 /* get maximum possible bytes to batch per sample */
5978 /* get minimum delay for each requested sensor */
5979 ssize_t nBytes = 0;
5980 int64_t wanted = 1000000000LL, ns = 0;
5981 int64_t timeoutInMs = 0;
5982 for (int i = 0; i < NumSensors; i++) {
5983 if (batchMode == 1) {
5984 ns = mBatchDelays[i];
5985 LOGV_IF(DEBUG_BATCHING && EXTRA_VERBOSE,
5986 "HAL:batch - requested sensor=0x%01x, batch delay=%lld", mEnabled & (1 << i), ns);
5987 // take the min delay ==> max rate
5988 wanted = (ns < wanted) ? ns : wanted;
5989 if (i <= RawMagneticField) {
5990 nBytes += 8;
5991 }
5992 #ifdef ENABLE_PRESSURE
5993 if (i == Pressure) {
5994 nBytes += 6;
5995 }
5996 #endif
5997 if ((i == StepDetector) || (i == GameRotationVector)) {
5998 nBytes += 16;
5999 }
6000 }
6001 }
6002
6003 /* starting from code below, we will modify hardware */
6004 /* first edit global batch mode mask */
6005
6006 if (!timeout) {
6007 mBatchEnabled &= ~(1 << what);
6008 mBatchDelays[what] = 1000000000LL;
6009 mDelays[what] = period_ns;
6010 mBatchTimeouts[what] = 100000000000LL;
6011 } else {
6012 mBatchEnabled |= (1 << what);
6013 mBatchDelays[what] = period_ns;
6014 mDelays[what] = period_ns;
6015 mBatchTimeouts[what] = timeout;
6016 }
6017
6018 // Check if need to change configurations
6019 int master_enable_call = 0;
6020 int64_t tmp_batch_timeout = 0;
6021 bool tmp_dmp_state = 0;
6022 int64_t tmp_gyro_rate;
6023 int64_t tmp_accel_rate;
6024 int64_t tmp_compass_rate;
6025 int64_t tmp_pressure_rate;
6026 int64_t tmp_quat_rate;
6027 int64_t tmp_reset_rate;
6028 bool skip_reset_data_rate = false;
6029
6030 if (mFirstBatchCall) {
6031 LOGI_IF(0, "HAL: mFirstBatchCall = %d", mFirstBatchCall);
6032 master_enable_call++;
6033 mFirstBatchCall = 0;
6034 }
6035
6036 if (mEnableCalled) {
6037 LOGI_IF(0, "HAL: mEnableCalled = %d", mEnableCalled);
6038 master_enable_call++;
6039 mEnableCalled = 0;
6040 }
6041
6042 if(((int)mOldBatchEnabledMask != batchMode) || batchMode) {
6043 calcBatchTimeout(batchMode, &tmp_batch_timeout);
6044 if (tmp_batch_timeout != mBatchTimeoutInMs)
6045 master_enable_call++;
6046 if (computeDmpState(&tmp_dmp_state) < 0) {
6047 LOGE("HAL:ERR can't compute dmp state");
6048 }
6049 if (tmp_dmp_state != mDmpState)
6050 master_enable_call++;
6051 }
6052
6053 if (batchMode == 1) {
6054 if (calcBatchDataRates(&tmp_gyro_rate, &tmp_accel_rate, &tmp_compass_rate, &tmp_pressure_rate, &tmp_quat_rate) < 0) {
6055 LOGE("HAL:ERR can't get batch data rates");
6056 }
6057 if (tmp_gyro_rate != mGyroBatchRate)
6058 master_enable_call++;
6059 if (tmp_accel_rate != mAccelBatchRate)
6060 master_enable_call++;
6061 if (tmp_compass_rate != mCompassBatchRate)
6062 master_enable_call++;
6063 if (tmp_pressure_rate != mPressureBatchRate)
6064 master_enable_call++;
6065 if (tmp_quat_rate != mQuatBatchRate)
6066 master_enable_call++;
6067 } else {
6068 if (calctDataRates(&tmp_reset_rate, &tmp_gyro_rate, &tmp_accel_rate, &tmp_compass_rate, &tmp_pressure_rate) < 0) {
6069 skip_reset_data_rate = true;
6070 LOGV_IF(ENG_VERBOSE, "HAL:ERR can't get output rate back to original setting");
6071 }
6072 if (tmp_reset_rate != mResetRate)
6073 master_enable_call++;
6074 if (tmp_gyro_rate != mGyroRate)
6075 master_enable_call++;
6076 if (tmp_accel_rate != mAccelRate)
6077 master_enable_call++;
6078 if (tmp_compass_rate != mCompassRate)
6079 master_enable_call++;
6080 if (tmp_pressure_rate != mPressureRate)
6081 master_enable_call++;
6082 }
6083 uint32_t dataInterrupt = (mEnabled || (mFeatureActiveMask & INV_DMP_BATCH_MODE));
6084 if (dataInterrupt != mDataInterrupt)
6085 master_enable_call++;
6086
6087 if (master_enable_call == 0) {
6088 LOGI_IF(0, "HAL: Skip batch configurations");
6089 goto batch_end;
6090 } else {
6091 LOGI_IF(0, "HAL: Do batch configurations");
6092 }
6093
6094
6095 // reset master enable
6096 res = masterEnable(0);
6097 if (res < 0) {
6098 return res;
6099 }
6100
6101 if(((int)mOldBatchEnabledMask != batchMode) || batchMode) {
6102
6103 /* remember batch mode that is set */
6104 mOldBatchEnabledMask = batchMode;
6105
6106 /* For these sensors, switch to different data output */
6107 int featureMask = computeBatchDataOutput();
6108
6109 LOGV_IF(ENG_VERBOSE, "batchMode =%d, featureMask=0x%x, mEnabled=%d",
6110 batchMode, featureMask, mEnabled);
6111 if (DEBUG_BATCHING && EXTRA_VERBOSE) {
6112 LOGV("HAL:batch - sensor=0x%01x", mBatchEnabled);
6113 for (int d = 0; d < NumSensors; d++) {
6114 LOGV("HAL:batch - sensor status=0x%01x batch status=0x%01x timeout=%lld delay=%lld",
6115 mEnabled & (1 << d), (mBatchEnabled & (1 << d)), mBatchTimeouts[d],
6116 mBatchDelays[d]);
6117 }
6118 }
6119
6120 /* case for Ped standalone */
6121 if ((batchMode == 1) && (featureMask & INV_DMP_PED_STANDALONE) &&
6122 (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
6123 LOGI_IF(ENG_VERBOSE, "batch - ID_P only = 0x%x", mBatchEnabled);
6124 enablePedQuaternion(0);
6125 enablePedStandalone(1);
6126 } else {
6127 enablePedStandalone(0);
6128 if (featureMask & INV_DMP_PED_QUATERNION) {
6129 enableLPQuaternion(0);
6130 enablePedQuaternion(1);
6131 }
6132 }
6133
6134 /* case for Ped Quaternion */
6135 if ((batchMode == 1) && (featureMask & INV_DMP_PED_QUATERNION) &&
6136 (mEnabled & (1 << GameRotationVector)) &&
6137 (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
6138 LOGI_IF(ENG_VERBOSE, "batch - ID_P and GRV or ALL = 0x%x", mBatchEnabled);
6139 LOGI_IF(ENG_VERBOSE, "batch - ID_P is enabled for batching, PED quat will be automatically enabled");
6140 enableLPQuaternion(0);
6141 enablePedQuaternion(1);
6142
6143 /* set pedq rate */
6144 wanted = mBatchDelays[GameRotationVector];
6145 setPedQuaternionRate(wanted);
6146 } else if (!(featureMask & INV_DMP_PED_STANDALONE)){
6147 LOGV_IF(ENG_VERBOSE, "batch - PedQ Toggle back to normal 6 axis");
6148 if (mEnabled & (1 << GameRotationVector)) {
6149 enableLPQuaternion(checkLPQRateSupported());
6150 }
6151 enablePedQuaternion(0);
6152 } else {
6153 enablePedQuaternion(0);
6154 }
6155
6156 /* case for Ped indicator */
6157 if ((batchMode == 1) && ((featureMask & INV_DMP_PED_INDICATOR))) {
6158 enablePedIndicator(1);
6159 } else {
6160 enablePedIndicator(0);
6161 }
6162
6163 /* case for Six Axis Quaternion */
6164 if ((batchMode == 1) && (featureMask & INV_DMP_6AXIS_QUATERNION) &&
6165 (mEnabled & (1 << GameRotationVector))) {
6166 LOGI_IF(ENG_VERBOSE, "batch - GRV = 0x%x", mBatchEnabled);
6167 enableLPQuaternion(0);
6168 enable6AxisQuaternion(1);
6169 if (what == GameRotationVector) {
6170 setInitial6QuatValue();
6171 }
6172
6173 /* set sixaxis rate */
6174 wanted = mBatchDelays[GameRotationVector];
6175 set6AxisQuaternionRate(wanted);
6176 } else if (!(featureMask & INV_DMP_PED_QUATERNION)){
6177 LOGV_IF(ENG_VERBOSE, "batch - 6Axis Toggle back to normal 6 axis");
6178 if (mEnabled & (1 << GameRotationVector)) {
6179 enableLPQuaternion(checkLPQRateSupported());
6180 }
6181 enable6AxisQuaternion(0);
6182 } else {
6183 enable6AxisQuaternion(0);
6184 }
6185
6186 /* TODO: This may make a come back some day */
6187 /* Not to overflow hardware FIFO if flag is set */
6188 /*if (flags & (1 << SENSORS_BATCH_WAKE_UPON_FIFO_FULL)) {
6189 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
6190 0, mpu.batchmode_wake_fifo_full_on, getTimestamp());
6191 if (write_sysfs_int(mpu.batchmode_wake_fifo_full_on, 0) < 0) {
6192 LOGE("HAL:ERR can't write batchmode_wake_fifo_full_on");
6193 }
6194 }*/
6195
6196 writeBatchTimeout(batchMode, tmp_batch_timeout);
6197
6198 if (SetDmpState(tmp_dmp_state) < 0) {
6199 LOGE("HAL:ERR can't set dmp state");
6200 }
6201
6202 }//end of batch mode modify
6203
6204 if (batchMode == 1) {
6205 /* set batch rates */
6206 if (setBatchDataRates(tmp_gyro_rate, tmp_accel_rate, tmp_compass_rate, tmp_pressure_rate, tmp_quat_rate) < 0) {
6207 LOGE("HAL:ERR can't set batch data rates");
6208 }
6209 } else {
6210 /* reset sensor rate */
6211 if (!skip_reset_data_rate) {
6212 if (resetDataRates(tmp_reset_rate, tmp_gyro_rate, tmp_accel_rate, tmp_compass_rate, tmp_pressure_rate) < 0) {
6213 LOGE("HAL:ERR can't reset output rate back to original setting");
6214 }
6215 }
6216 }
6217
6218 // set sensor data interrupt
6219 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
6220 !dataInterrupt, mpu.dmp_event_int_on, getTimestamp());
6221 if (write_sysfs_int(mpu.dmp_event_int_on, !dataInterrupt) < 0) {
6222 res = -1;
6223 LOGE("HAL:ERR can't enable DMP event interrupt");
6224 }
6225 mDataInterrupt = dataInterrupt;
6226
6227 if (enabled_sensors || mFeatureActiveMask) {
6228 masterEnable(1);
6229 }
6230
6231 batch_end:
6232 return res;
6233 }
6234
6235 /* Send empty event when: */
6236 /* 1. batch mode is not enabled */
6237 /* 2. no data in HW FIFO */
6238 /* return status zero if (2) */
6239 int MPLSensor::flush(int handle)
6240 {
6241 VFUNC_LOG;
6242
6243 int res = 0;
6244 int status = 0;
6245 android::String8 sname;
6246 int what = -1;
6247
6248 getHandle(handle, what, sname);
6249 if (uint32_t(what) >= NumSensors) {
6250 LOGE("HAL:flush - what=%d is invalid", what);
6251 return -EINVAL;
6252 }
6253
6254 LOGV_IF(PROCESS_VERBOSE, "HAL: flush - select sensor %s (handle %d)", sname.string(), handle);
6255
6256
6257 if (((what != StepDetector) && (!(mEnabled & (1 << what)))) ||
6258 ((what == StepDetector) && !(mFeatureActiveMask & INV_DMP_PEDOMETER))) {
6259 LOGV_IF(ENG_VERBOSE, "HAL: flush - sensor %s not enabled", sname.string());
6260 return -EINVAL;
6261 }
6262
6263 if(!(mBatchEnabled & (1 << what))) {
6264 LOGV_IF(PROCESS_VERBOSE, "HAL:flush - batch mode not enabled for sensor %s (handle %d)", sname.string(), handle);
6265 }
6266
6267 mFlushSensorEnabledVector.push_back(handle);
6268
6269 /*write sysfs */
6270 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
6271 mpu.flush_batch, getTimestamp());
6272
6273 status = read_sysfs_int(mpu.flush_batch, &res);
6274
6275 if (status < 0)
6276 LOGE("HAL: flush - error invoking flush_batch");
6277
6278 /* driver returns 0 if FIFO is empty */
6279 if (res == 0) {
6280 LOGV_IF(ENG_VERBOSE, "HAL: flush - no data in FIFO");
6281 }
6282
6283 LOGV_IF(ENG_VERBOSE, "HAl:flush - mFlushSensorEnabledVector=%d res=%d status=%d", handle, res, status);
6284
6285 return 0;
6286 }
6287
6288 int MPLSensor::selectAndSetQuaternion(int batchMode, int mEnabled, long long featureMask)
6289 {
6290 VFUNC_LOG;
6291 int res = 0;
6292
6293 int64_t wanted;
6294
6295 /* case for Ped Quaternion */
6296 if (batchMode == 1) {
6297 if ((featureMask & INV_DMP_PED_QUATERNION) &&
6298 (mEnabled & (1 << GameRotationVector)) &&
6299 (mFeatureActiveMask & INV_DMP_PEDOMETER)) {
6300 enableLPQuaternion(0);
6301 enable6AxisQuaternion(0);
6302 setInitial6QuatValue();
6303 enablePedQuaternion(1);
6304
6305 /* set pedq rate */
6306 wanted = mBatchDelays[GameRotationVector];
6307 setPedQuaternionRate(wanted);
6308 } else if ((featureMask & INV_DMP_6AXIS_QUATERNION) &&
6309 (mEnabled & (1 << GameRotationVector))) {
6310 enableLPQuaternion(0);
6311 enablePedQuaternion(0);
6312 setInitial6QuatValue();
6313 enable6AxisQuaternion(1);
6314
6315 /* set sixaxis rate */
6316 wanted = mBatchDelays[GameRotationVector];
6317 set6AxisQuaternionRate(wanted);
6318 } else {
6319 enablePedQuaternion(0);
6320 enable6AxisQuaternion(0);
6321 }
6322 } else {
6323 if(mEnabled & (1 << GameRotationVector)) {
6324 enablePedQuaternion(0);
6325 enable6AxisQuaternion(0);
6326 enableLPQuaternion(checkLPQRateSupported());
6327 }
6328 else {
6329 enablePedQuaternion(0);
6330 enable6AxisQuaternion(0);
6331 }
6332 }
6333
6334 return res;
6335 }
6336
6337 /*
6338 Select Quaternion and Options for Batching
6339
6340 ID_P ID_GRV HW Batch Type
6341 a 1 1 1 PedQ, Ped Indicator, HW
6342 b 1 1 0 PedQ
6343 c 1 0 1 Ped Indicator, HW
6344 d 1 0 0 Ped Standalone, Ped Indicator
6345 e 0 1 1 6Q, HW
6346 f 0 1 0 6Q
6347 g 0 0 1 HW
6348 h 0 0 0 LPQ <defualt>
6349 */
6350 int MPLSensor::computeBatchDataOutput()
6351 {
6352 VFUNC_LOG;
6353
6354 int featureMask = 0;
6355 if (mBatchEnabled == 0)
6356 return 0;//h
6357
6358 uint32_t hardwareSensorMask = (1 << Gyro)
6359 | (1 << RawGyro)
6360 | (1 << Accelerometer)
6361 | (1 << MagneticField)
6362 #ifdef ENABLE_PRESSURE
6363 | (1 << Pressure)
6364 #endif
6365 | (1 << RawMagneticField);
6366
6367 LOGV_IF(ENG_VERBOSE, "hardwareSensorMask = 0x%0x, mBatchEnabled = 0x%0x",
6368 hardwareSensorMask, mBatchEnabled);
6369
6370 if (mBatchEnabled & (1 << StepDetector)) {
6371 if (mBatchEnabled & (1 << GameRotationVector)) {
6372 if ((mBatchEnabled & hardwareSensorMask)) {
6373 featureMask |= INV_DMP_6AXIS_QUATERNION;//a
6374 featureMask |= INV_DMP_PED_INDICATOR;
6375 //LOGE("batch output: a");
6376 } else {
6377 featureMask |= INV_DMP_PED_QUATERNION; //b
6378 featureMask |= INV_DMP_PED_INDICATOR; //always piggy back a bit
6379 //LOGE("batch output: b");
6380 }
6381 } else {
6382 if (mBatchEnabled & hardwareSensorMask) {
6383 featureMask |= INV_DMP_PED_INDICATOR; //c
6384 //LOGE("batch output: c");
6385 } else {
6386 featureMask |= INV_DMP_PED_STANDALONE; //d
6387 featureMask |= INV_DMP_PED_INDICATOR; //required for standalone
6388 //LOGE("batch output: d");
6389 }
6390 }
6391 } else if (mBatchEnabled & (1 << GameRotationVector)) {
6392 featureMask |= INV_DMP_6AXIS_QUATERNION; //e,f
6393 //LOGE("batch output: e,f");
6394 } else {
6395 LOGV_IF(ENG_VERBOSE,
6396 "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask);
6397 //LOGE("batch output: g");
6398 return 0; //g
6399 }
6400
6401 LOGV_IF(ENG_VERBOSE,
6402 "HAL:computeBatchDataOutput: featuerMask=0x%x", featureMask);
6403 return featureMask;
6404 }
6405
6406 int MPLSensor::getDmpPedometerFd()
6407 {
6408 VFUNC_LOG;
6409 LOGV_IF(EXTRA_VERBOSE, "getDmpPedometerFd returning %d", dmp_pedometer_fd);
6410 return dmp_pedometer_fd;
6411 }
6412
6413 /* @param [in] : outputType = 1 --event is from PED_Q */
6414 /* outputType = 0 --event is from ID_SC, ID_P */
6415 int MPLSensor::readDmpPedometerEvents(sensors_event_t* data, int count,
6416 int32_t id, int outputType)
6417 {
6418 VFUNC_LOG;
6419
6420 int res = 0;
6421 char dummy[4];
6422
6423 int numEventReceived = 0;
6424 int update = 0;
6425
6426 LOGI_IF(0, "HAL: Read Pedometer Event ID=%d", id);
6427 switch (id) {
6428 case ID_P:
6429 if (mDmpPedometerEnabled && count > 0) {
6430 /* Handles return event */
6431 LOGI("HAL: Step detected");
6432 update = sdHandler(&mSdEvents);
6433 }
6434
6435 if (update && count > 0) {
6436 *data++ = mSdEvents;
6437 count--;
6438 numEventReceived++;
6439 }
6440 break;
6441 case ID_SC:
6442 FILE *fp;
6443 uint64_t stepCount;
6444 uint64_t stepCountTs;
6445
6446 if (mDmpStepCountEnabled && count > 0) {
6447 fp = fopen(mpu.pedometer_steps, "r");
6448 if (fp == NULL) {
6449 LOGE("HAL:cannot open pedometer_steps");
6450 } else {
6451 if (fscanf(fp, "%lld\n", &stepCount) < 0) {
6452 LOGV_IF(PROCESS_VERBOSE, "HAL:cannot read pedometer_steps");
6453 if (fclose(fp) < 0) {
6454 LOGW("HAL:cannot close pedometer_steps");
6455 }
6456 return 0;
6457 }
6458 if (fclose(fp) < 0) {
6459 LOGW("HAL:cannot close pedometer_steps");
6460 }
6461 }
6462
6463 /* return event onChange only */
6464 if (stepCount == mLastStepCount) {
6465 return 0;
6466 }
6467
6468 mLastStepCount = stepCount;
6469
6470 /* Read step count timestamp */
6471 fp = fopen(mpu.pedometer_counter, "r");
6472 if (fp == NULL) {
6473 LOGE("HAL:cannot open pedometer_counter");
6474 } else{
6475 if (fscanf(fp, "%lld\n", &stepCountTs) < 0) {
6476 LOGE("HAL:cannot read pedometer_counter");
6477 if (fclose(fp) < 0) {
6478 LOGE("HAL:cannot close pedometer_counter");
6479 }
6480 return 0;
6481 }
6482 if (fclose(fp) < 0) {
6483 LOGE("HAL:cannot close pedometer_counter");
6484 return 0;
6485 }
6486 }
6487 mScEvents.timestamp = stepCountTs;
6488
6489 /* Handles return event */
6490 update = scHandler(&mScEvents);
6491 }
6492
6493 if (update && count > 0) {
6494 *data++ = mScEvents;
6495 count--;
6496 numEventReceived++;
6497 }
6498 break;
6499 }
6500
6501 if (!outputType) {
6502 // read dummy data per driver's request
6503 // only required if actual irq is issued
6504 read(dmp_pedometer_fd, dummy, 4);
6505 } else {
6506 return 1;
6507 }
6508
6509 return numEventReceived;
6510 }
6511
6512 int MPLSensor::getDmpSignificantMotionFd()
6513 {
6514 VFUNC_LOG;
6515
6516 LOGV_IF(EXTRA_VERBOSE, "getDmpSignificantMotionFd returning %d",
6517 dmp_sign_motion_fd);
6518 return dmp_sign_motion_fd;
6519 }
6520
6521 int MPLSensor::readDmpSignificantMotionEvents(sensors_event_t* data, int count)
6522 {
6523 VFUNC_LOG;
6524
6525 int res = 0;
6526 char dummy[4];
6527 int vibrator = 0;
6528 FILE *fp;
6529 int sensors = mEnabled;
6530 int numEventReceived = 0;
6531 int update = 0;
6532 static int64_t lastVibTrigger = 0;
6533
6534 if (mDmpSignificantMotionEnabled && count > 0) {
6535
6536 // If vibrator is going off, ignore this event
6537 fp = fopen(VIBRATOR_ENABLE_FILE, "r");
6538 if (fp != NULL) {
6539 if (fscanf(fp, "%d\n", &vibrator) < 0) {
6540 LOGE("HAL:cannot read %s", VIBRATOR_ENABLE_FILE);
6541 }
6542 if (fclose(fp) < 0) {
6543 LOGE("HAL:cannot close %s", VIBRATOR_ENABLE_FILE);
6544 }
6545 if (vibrator != 0) {
6546 lastVibTrigger = android::elapsedRealtimeNano();
6547 LOGV_IF(ENG_VERBOSE, "SMD triggered by vibrator, ignoring SMD event");
6548 return 0;
6549 } else if (lastVibTrigger) {
6550 // vibrator recently triggered SMD, discard related events
6551 int64_t now = android::elapsedRealtimeNano();
6552 if ((now - lastVibTrigger) < MIN_TRIGGER_TIME_AFTER_VIBRATOR_NS) {
6553 LOGV_IF(ENG_VERBOSE, "HAL: SMD triggered too close to vibrator (delta %lldnS), ignoring",
6554 (now-lastVibTrigger));
6555 return 0;
6556 } else {
6557 LOGV_IF(ENG_VERBOSE, "HAL: SMD triggered %lld after vibrator (last %lld now %lld)",
6558 now-lastVibTrigger, lastVibTrigger, now);
6559 lastVibTrigger = 0;
6560 }
6561 }
6562 } else {
6563 LOGE("HAL:cannot open %s", VIBRATOR_ENABLE_FILE);
6564 }
6565
6566 /* By implementation, smd is disabled once an event is triggered */
6567 sensors_event_t temp;
6568
6569 /* Handles return event */
6570 LOGI("HAL: SMD detected");
6571 int update = smHandler(&mSmEvents);
6572 if (update && count > 0) {
6573 *data++ = mSmEvents;
6574 count--;
6575 numEventReceived++;
6576
6577 /* reset smd state */
6578 mDmpSignificantMotionEnabled = 0;
6579 mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION;
6580
6581 /* auto disable this sensor */
6582 enableDmpSignificantMotion(0);
6583 }
6584 }
6585
6586 // read dummy data per driver's request
6587 read(dmp_sign_motion_fd, dummy, 4);
6588
6589 return numEventReceived;
6590 }
6591
6592 int MPLSensor::enableDmpSignificantMotion(int en)
6593 {
6594 VFUNC_LOG;
6595
6596 int res = 0;
6597 int enabled_sensors = mEnabled;
6598
6599 if (isMpuNonDmp())
6600 return res;
6601
6602 // reset master enable
6603 res = masterEnable(0);
6604 if (res < 0)
6605 return res;
6606
6607 //Toggle significant montion detection
6608 if(en) {
6609 LOGV_IF(ENG_VERBOSE, "HAL:Enabling Significant Motion");
6610 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
6611 1, mpu.smd_enable, getTimestamp());
6612 if (write_sysfs_int(mpu.smd_enable, 1) < 0) {
6613 LOGE("HAL:ERR can't write DMP smd_enable");
6614 res = -1; //Indicate an err
6615 }
6616 mFeatureActiveMask |= INV_DMP_SIGNIFICANT_MOTION;
6617 }
6618 else {
6619 LOGV_IF(ENG_VERBOSE, "HAL:Disabling Significant Motion");
6620 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
6621 0, mpu.smd_enable, getTimestamp());
6622 if (write_sysfs_int(mpu.smd_enable, 0) < 0) {
6623 LOGE("HAL:ERR write DMP smd_enable");
6624 }
6625 mFeatureActiveMask &= ~INV_DMP_SIGNIFICANT_MOTION;
6626 }
6627
6628 if ((res = setDmpFeature(en)) < 0)
6629 return res;
6630
6631 if ((res = computeAndSetDmpState()) < 0)
6632 return res;
6633
6634 if (!mBatchEnabled && (resetDataRates() < 0))
6635 return res;
6636
6637 if(en || enabled_sensors || mFeatureActiveMask) {
6638 res = masterEnable(1);
6639 }
6640 return res;
6641 }
6642
6643 void MPLSensor::setInitial6QuatValue()
6644 {
6645 VFUNC_LOG;
6646
6647 if (!mInitial6QuatValueAvailable)
6648 return;
6649
6650 /* convert to unsigned char array */
6651 size_t length = 16;
6652 unsigned char quat[16];
6653 convert_long_to_hex_char(mInitial6QuatValue, quat, 4);
6654
6655 /* write to sysfs */
6656 LOGV_IF(EXTRA_VERBOSE, "HAL:sysfs:echo quat value > %s", mpu.six_axis_q_value);
6657 LOGV_IF(EXTRA_VERBOSE && ENG_VERBOSE, "quat=%ld,%ld,%ld,%ld", mInitial6QuatValue[0],
6658 mInitial6QuatValue[1],
6659 mInitial6QuatValue[2],
6660 mInitial6QuatValue[3]);
6661 FILE* fptr = fopen(mpu.six_axis_q_value, "w");
6662 if(fptr == NULL) {
6663 LOGE("HAL:could not open six_axis_q_value");
6664 } else {
6665 if (fwrite(quat, 1, length, fptr) != length) {
6666 LOGE("HAL:write six axis q value failed");
6667 } else {
6668 mInitial6QuatValueAvailable = 0;
6669 }
6670 if (fclose(fptr) < 0) {
6671 LOGE("HAL:could not close six_axis_q_value");
6672 }
6673 }
6674
6675 return;
6676 }
6677 int MPLSensor::writeSignificantMotionParams(bool toggleEnable,
6678 uint32_t delayThreshold1,
6679 uint32_t delayThreshold2,
6680 uint32_t motionThreshold)
6681 {
6682 VFUNC_LOG;
6683
6684 int res = 0;
6685
6686 // Turn off enable
6687 if (toggleEnable) {
6688 masterEnable(0);
6689 }
6690
6691 // Write supplied values
6692 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
6693 delayThreshold1, mpu.smd_delay_threshold, getTimestamp());
6694 res = write_sysfs_int(mpu.smd_delay_threshold, delayThreshold1);
6695 if (res == 0) {
6696 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
6697 delayThreshold2, mpu.smd_delay_threshold2, getTimestamp());
6698 res = write_sysfs_int(mpu.smd_delay_threshold2, delayThreshold2);
6699 }
6700 if (res == 0) {
6701 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
6702 motionThreshold, mpu.smd_threshold, getTimestamp());
6703 res = write_sysfs_int(mpu.smd_threshold, motionThreshold);
6704 }
6705
6706 // Turn on enable
6707 if (toggleEnable) {
6708 masterEnable(1);
6709 }
6710 return res;
6711 }
6712
6713 int MPLSensor::calcBatchDataRates(int64_t *gyro_rate, int64_t *accel_rate, int64_t *compass_rate, int64_t *pressure_rate, int64_t *quat_rate)
6714 {
6715 VFUNC_LOG;
6716
6717 int res = 0;
6718 int tempFd = -1;
6719
6720 int64_t gyroRate;
6721 int64_t accelRate;
6722 int64_t compassRate;
6723 #ifdef ENABLE_PRESSURE
6724 int64_t pressureRate;
6725 #endif
6726 int64_t quatRate = 0;
6727
6728 int mplGyroRate;
6729 int mplAccelRate;
6730 int mplCompassRate;
6731 int mplQuatRate;
6732
6733 #ifdef ENABLE_MULTI_RATE
6734 gyroRate = mBatchDelays[Gyro];
6735 /* take care of case where only one type of gyro sensors or
6736 compass sensors is turned on */
6737 if (mBatchEnabled & (1 << Gyro) || mBatchEnabled & (1 << RawGyro)) {
6738 gyroRate = (mBatchDelays[Gyro] <= mBatchDelays[RawGyro]) ?
6739 (mBatchEnabled & (1 << Gyro) ? mBatchDelays[Gyro] : mBatchDelays[RawGyro]):
6740 (mBatchEnabled & (1 << RawGyro) ? mBatchDelays[RawGyro] : mBatchDelays[Gyro]);
6741 }
6742 compassRate = mBatchDelays[MagneticField];
6743 if (mBatchEnabled & (1 << MagneticField) || mBatchEnabled & (1 << RawMagneticField)) {
6744 compassRate = (mBatchDelays[MagneticField] <= mBatchDelays[RawMagneticField]) ?
6745 (mBatchEnabled & (1 << MagneticField) ? mBatchDelays[MagneticField] :
6746 mBatchDelays[RawMagneticField]) :
6747 (mBatchEnabled & (1 << RawMagneticField) ? mBatchDelays[RawMagneticField] :
6748 mBatchDelays[MagneticField]);
6749 }
6750 accelRate = mBatchDelays[Accelerometer];
6751 #ifdef ENABLE_PRESSURE
6752 pressureRate = mBatchDelays[Pressure];
6753 #endif //ENABLE_PRESSURE
6754
6755 if ((mFeatureActiveMask & INV_DMP_PED_QUATERNION) ||
6756 (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)) {
6757 quatRate = mBatchDelays[GameRotationVector];
6758 mplQuatRate = (int) quatRate / 1000LL;
6759 inv_set_quat_sample_rate(mplQuatRate);
6760 inv_set_rotation_vector_6_axis_sample_rate(mplQuatRate);
6761 LOGV_IF(PROCESS_VERBOSE,
6762 "HAL:MPL rv 6 axis sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplQuatRate,
6763 1000000000.f / quatRate );
6764 LOGV_IF(PROCESS_VERBOSE,
6765 "HAL:MPL quat sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplQuatRate,
6766 1000000000.f / quatRate );
6767 //getDmpRate(&quatRate);
6768 }
6769
6770 mplGyroRate = (int) gyroRate / 1000LL;
6771 mplAccelRate = (int) accelRate / 1000LL;
6772 mplCompassRate = (int) compassRate / 1000LL;
6773
6774 /* set rate in MPL */
6775 /* compass can only do 100Hz max */
6776 inv_set_gyro_sample_rate(mplGyroRate);
6777 inv_set_accel_sample_rate(mplAccelRate);
6778 inv_set_compass_sample_rate(mplCompassRate);
6779
6780 LOGV_IF(PROCESS_VERBOSE,
6781 "HAL:MPL gyro sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplGyroRate, 1000000000.f / gyroRate);
6782 LOGV_IF(PROCESS_VERBOSE,
6783 "HAL:MPL accel sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplAccelRate, 1000000000.f / accelRate);
6784 LOGV_IF(PROCESS_VERBOSE,
6785 "HAL:MPL compass sample rate: (mpl)=%d us (mpu)=%.2f Hz", mplCompassRate, 1000000000.f / compassRate);
6786
6787 #else
6788 /* search the minimum delay requested across all enabled sensors */
6789 int64_t wanted = 1000000000LL;
6790 for (int i = 0; i < NumSensors; i++) {
6791 if (mBatchEnabled & (1 << i)) {
6792 int64_t ns = mBatchDelays[i];
6793 wanted = wanted < ns ? wanted : ns;
6794 }
6795 }
6796 gyroRate = wanted;
6797 accelRate = wanted;
6798 compassRate = wanted;
6799 #ifdef ENABLE_PRESSURE
6800 pressureRate = wanted;
6801 #endif
6802 #endif
6803
6804 *gyro_rate = gyroRate;
6805 *accel_rate = accelRate;
6806 *compass_rate = compassRate;
6807 #ifdef ENABLE_PRESSURE
6808 *pressure_rate = pressureRate;
6809 #endif
6810 *quat_rate = quatRate;
6811
6812 return 0;
6813 }
6814
6815 int MPLSensor::MPLSensor::setBatchDataRates(int64_t gyroRate, int64_t accelRate, int64_t compassRate, int64_t pressureRate, int64_t quatRate)
6816 {
6817 VFUNC_LOG;
6818
6819 int res = 0;
6820 int tempFd = -1;
6821
6822 if ((mFeatureActiveMask & INV_DMP_PED_QUATERNION) ||
6823 (mFeatureActiveMask & INV_DMP_6AXIS_QUATERNION)) {
6824 getDmpRate(&quatRate);
6825 }
6826
6827 /* takes care of gyro rate */
6828 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
6829 1000000000.f / gyroRate, mpu.gyro_rate,
6830 getTimestamp());
6831 tempFd = open(mpu.gyro_rate, O_RDWR);
6832 res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
6833 if(res < 0) {
6834 LOGE("HAL:GYRO update delay error");
6835 }
6836
6837 /* takes care of accel rate */
6838 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
6839 1000000000.f / accelRate, mpu.accel_rate,
6840 getTimestamp());
6841 tempFd = open(mpu.accel_rate, O_RDWR);
6842 res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
6843 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
6844
6845 /* takes care of compass rate */
6846 if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
6847 compassRate = mCompassSensor->getMinDelay() * 1000LL;
6848 }
6849 mCompassSensor->setDelay(ID_M, compassRate);
6850
6851 #ifdef ENABLE_PRESSURE
6852 /* takes care of pressure rate */
6853 mPressureSensor->setDelay(ID_PS, pressureRate);
6854 #endif
6855
6856 mGyroBatchRate = gyroRate;
6857 mAccelBatchRate = accelRate;
6858 mCompassBatchRate = compassRate;
6859 mPressureBatchRate = pressureRate;
6860 mQuatBatchRate = quatRate;
6861
6862 return res;
6863 }
6864
6865 /* set batch data rate */
6866 /* this function should be optimized */
6867 int MPLSensor::setBatchDataRates()
6868 {
6869 VFUNC_LOG;
6870
6871 int res = 0;
6872
6873 int64_t gyroRate;
6874 int64_t accelRate;
6875 int64_t compassRate;
6876 int64_t pressureRate;
6877 int64_t quatRate;
6878
6879 calcBatchDataRates(&gyroRate, &accelRate, &compassRate, &pressureRate, &quatRate);
6880 setBatchDataRates(gyroRate, accelRate, compassRate, pressureRate, quatRate);
6881
6882 return res;
6883 }
6884
6885 int MPLSensor::calctDataRates(int64_t *resetRate, int64_t *gyroRate, int64_t *accelRate, int64_t *compassRate, int64_t *pressureRate)
6886 {
6887 VFUNC_LOG;
6888
6889 int res = 0;
6890 int tempFd = -1;
6891 int64_t wanted = 1000000000LL;
6892
6893 if (!mEnabled) {
6894 LOGV_IF(ENG_VERBOSE, "skip resetDataRates");
6895 return -1;
6896 }
6897 /* search the minimum delay requested across all enabled sensors */
6898 /* skip setting rates if it is not changed */
6899 for (int i = 0; i < NumSensors; i++) {
6900 if (mEnabled & (1 << i)) {
6901 int64_t ns = mDelays[i];
6902 #ifdef ENABLE_PRESSURE
6903 if ((wanted == ns) && (i != Pressure)) {
6904 LOGV_IF(ENG_VERBOSE, "skip resetDataRates : same delay mDelays[%d]=%lld", i,mDelays[i]);
6905 //return 0;
6906 }
6907 #endif
6908 LOGV_IF(ENG_VERBOSE, "resetDataRates - mDelays[%d]=%lld", i, mDelays[i]);
6909 wanted = wanted < ns ? wanted : ns;
6910 }
6911 }
6912
6913 *resetRate = wanted;
6914 *gyroRate = wanted;
6915 *accelRate = wanted;
6916 *compassRate = wanted;
6917 *pressureRate = wanted;
6918
6919 return 0;
6920 }
6921
6922 int MPLSensor::resetDataRates(int64_t resetRate, int64_t gyroRate, int64_t accelRate, int64_t compassRate, int64_t pressureRate)
6923 {
6924 VFUNC_LOG;
6925
6926 int res = 0;
6927 int tempFd = -1;
6928 int64_t wanted;
6929
6930 wanted = resetRate;
6931
6932 /* set mpl data rate */
6933 inv_set_gyro_sample_rate((int)gyroRate/1000LL);
6934 inv_set_accel_sample_rate((int)accelRate/1000LL);
6935 inv_set_compass_sample_rate((int)compassRate/1000LL);
6936 inv_set_linear_acceleration_sample_rate((int)resetRate/1000LL);
6937 inv_set_orientation_sample_rate((int)resetRate/1000LL);
6938 inv_set_rotation_vector_sample_rate((int)resetRate/1000LL);
6939 inv_set_gravity_sample_rate((int)resetRate/1000LL);
6940 inv_set_orientation_geomagnetic_sample_rate((int)resetRate/1000LL);
6941 inv_set_rotation_vector_6_axis_sample_rate((int)resetRate/1000LL);
6942 inv_set_geomagnetic_rotation_vector_sample_rate((int)resetRate/1000LL);
6943
6944 LOGV_IF(PROCESS_VERBOSE,
6945 "HAL:MPL gyro sample rate: (mpl)=%lld us (mpu)=%.2f Hz",
6946 gyroRate/1000LL, 1000000000.f / gyroRate);
6947 LOGV_IF(PROCESS_VERBOSE,
6948 "HAL:MPL accel sample rate: (mpl)=%lld us (mpu)=%.2f Hz",
6949 accelRate/1000LL, 1000000000.f / accelRate);
6950 LOGV_IF(PROCESS_VERBOSE,
6951 "HAL:MPL compass sample rate: (mpl)=%lld us (mpu)=%.2f Hz",
6952 compassRate/1000LL, 1000000000.f / compassRate);
6953
6954 /* reset dmp rate */
6955 getDmpRate (&wanted);
6956
6957 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
6958 1000000000.f / wanted, mpu.gyro_fifo_rate,
6959 getTimestamp());
6960 tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
6961 res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
6962 LOGE_IF(res < 0, "HAL:sampling frequency update delay error");
6963
6964 /* takes care of gyro rate */
6965 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
6966 1000000000.f / gyroRate, mpu.gyro_rate,
6967 getTimestamp());
6968 tempFd = open(mpu.gyro_rate, O_RDWR);
6969 res = write_attribute_sensor(tempFd, 1000000000.f / gyroRate);
6970 if(res < 0) {
6971 LOGE("HAL:GYRO update delay error");
6972 }
6973
6974 /* takes care of accel rate */
6975 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
6976 1000000000.f / accelRate, mpu.accel_rate,
6977 getTimestamp());
6978 tempFd = open(mpu.accel_rate, O_RDWR);
6979 res = write_attribute_sensor(tempFd, 1000000000.f / accelRate);
6980 LOGE_IF(res < 0, "HAL:ACCEL update delay error");
6981
6982 /* takes care of compass rate */
6983 if (compassRate < mCompassSensor->getMinDelay() * 1000LL) {
6984 compassRate = mCompassSensor->getMinDelay() * 1000LL;
6985 }
6986 mCompassSensor->setDelay(ID_M, compassRate);
6987
6988 #ifdef ENABLE_PRESSURE
6989 /* takes care of pressure rate */
6990 mPressureSensor->setDelay(ID_PS, pressureRate);
6991 #endif
6992
6993 /* takes care of lpq case for data rate at 200Hz */
6994 if (checkLPQuaternion()) {
6995 if (resetRate <= RATE_200HZ) {
6996 #ifndef USE_LPQ_AT_FASTEST
6997 enableLPQuaternion(0);
6998 #endif
6999 }
7000 }
7001
7002 mResetRate = resetRate;
7003 mGyroRate = gyroRate;
7004 mAccelRate = accelRate;
7005 mCompassRate = compassRate;
7006 mPressureRate = pressureRate;
7007
7008 return res;
7009 }
7010
7011 /* Set sensor rate */
7012 /* this function should be optimized */
7013 int MPLSensor::resetDataRates()
7014 {
7015 VFUNC_LOG;
7016
7017 int res = 0;
7018 int64_t resetRate;
7019 int64_t gyroRate;
7020 int64_t accelRate;
7021 int64_t compassRate;
7022 int64_t pressureRate;
7023
7024 res = calctDataRates(&resetRate, &gyroRate, &accelRate, &compassRate, &pressureRate);
7025 if (res)
7026 return 0;
7027
7028 resetDataRates(resetRate, gyroRate, accelRate, compassRate, pressureRate);
7029
7030 return res;
7031 }
7032
7033 void MPLSensor::resetMplStates()
7034 {
7035 VFUNC_LOG;
7036 LOGV_IF(ENG_VERBOSE, "HAL:resetMplStates()");
7037
7038 inv_gyro_was_turned_off();
7039 inv_accel_was_turned_off();
7040 inv_compass_was_turned_off();
7041 inv_quaternion_sensor_was_turned_off();
7042
7043 return;
7044 }
7045
7046 void MPLSensor::initBias()
7047 {
7048 VFUNC_LOG;
7049
7050 LOGV_IF(ENG_VERBOSE, "HAL:inititalize dmp and device offsets to 0");
7051 if(write_attribute_sensor_continuous(accel_x_dmp_bias_fd, 0) < 0) {
7052 LOGE("HAL:Error writing to accel_x_dmp_bias");
7053 }
7054 if(write_attribute_sensor_continuous(accel_y_dmp_bias_fd, 0) < 0) {
7055 LOGE("HAL:Error writing to accel_y_dmp_bias");
7056 }
7057 if(write_attribute_sensor_continuous(accel_z_dmp_bias_fd, 0) < 0) {
7058 LOGE("HAL:Error writing to accel_z_dmp_bias");
7059 }
7060
7061 if(write_attribute_sensor_continuous(accel_x_offset_fd, 0) < 0) {
7062 LOGE("HAL:Error writing to accel_x_offset");
7063 }
7064 if(write_attribute_sensor_continuous(accel_y_offset_fd, 0) < 0) {
7065 LOGE("HAL:Error writing to accel_y_offset");
7066 }
7067 if(write_attribute_sensor_continuous(accel_z_offset_fd, 0) < 0) {
7068 LOGE("HAL:Error writing to accel_z_offset");
7069 }
7070
7071 if(write_attribute_sensor_continuous(gyro_x_dmp_bias_fd, 0) < 0) {
7072 LOGE("HAL:Error writing to gyro_x_dmp_bias");
7073 }
7074 if(write_attribute_sensor_continuous(gyro_y_dmp_bias_fd, 0) < 0) {
7075 LOGE("HAL:Error writing to gyro_y_dmp_bias");
7076 }
7077 if(write_attribute_sensor_continuous(gyro_z_dmp_bias_fd, 0) < 0) {
7078 LOGE("HAL:Error writing to gyro_z_dmp_bias");
7079 }
7080
7081 if(write_attribute_sensor_continuous(gyro_x_offset_fd, 0) < 0) {
7082 LOGE("HAL:Error writing to gyro_x_offset");
7083 }
7084 if(write_attribute_sensor_continuous(gyro_y_offset_fd, 0) < 0) {
7085 LOGE("HAL:Error writing to gyro_y_offset");
7086 }
7087 if(write_attribute_sensor_continuous(gyro_z_offset_fd, 0) < 0) {
7088 LOGE("HAL:Error writing to gyro_z_offset");
7089 }
7090 return;
7091 }
7092
7093 /*TODO: reg_dump in a separate file*/
7094 void MPLSensor::sys_dump(bool fileMode)
7095 {
7096 VFUNC_LOG;
7097
7098 char sysfs_path[MAX_SYSFS_NAME_LEN];
7099 char scan_element_path[MAX_SYSFS_NAME_LEN];
7100
7101 memset(sysfs_path, 0, sizeof(sysfs_path));
7102 memset(scan_element_path, 0, sizeof(scan_element_path));
7103 inv_get_sysfs_path(sysfs_path);
7104 sprintf(scan_element_path, "%s%s", sysfs_path, "/scan_elements");
7105
7106 read_sysfs_dir(fileMode, sysfs_path);
7107 read_sysfs_dir(fileMode, scan_element_path);
7108
7109 dump_dmp_img("/data/local/read_img.h");
7110 return;
7111 }
7112