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