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