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