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