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, 0, 0, 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, 0, 0, 0, 0, {}}, 96 {"MPL Accelerometer", "Invensense", 1, 97 SENSORS_ACCELERATION_HANDLE, 98 SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 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, 0, 0, 0, 0, {}}, 102 {"MPL Orientation", "Invensense", 1, 103 SENSORS_ORIENTATION_HANDLE, 104 SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 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, 0, 0, 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, 0, 0, 0, 0, {}}, 111 {"MPL Gravity", "Invensense", 1, 112 SENSORS_GRAVITY_HANDLE, 113 SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 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, 0, 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 if (ns < 5000000LL) { 1504 ns = 5000000LL; 1505 } 1506 1507 /* store request rate to mDelays array for each sensor */ 1508 mDelays[what] = ns; 1509 1510 switch (what) { 1511 case Gyro: 1512 case RawGyro: 1513 case Accelerometer: 1514 for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated(); 1515 i++) { 1516 if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { 1517 LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i); 1518 return 0; 1519 } 1520 } 1521 break; 1522 1523 case MagneticField: 1524 if (mCompassSensor->isIntegrated() && 1525 (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) || 1526 ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) || 1527 ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) { 1528 LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel"); 1529 return 0; 1530 } 1531 break; 1532 1533 case Orientation: 1534 case RotationVector: 1535 case LinearAccel: 1536 case Gravity: 1537 if (isLowPowerQuatEnabled()) { 1538 LOGV_IF(PROCESS_VERBOSE, "HAL:need to update delay due to LPQ"); 1539 break; 1540 } 1541 1542 for (int i = 0; i < numSensors; i++) { 1543 if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { 1544 LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i); 1545 return 0; 1546 } 1547 } 1548 break; 1549 } 1550 1551 // pthread_mutex_lock(&mHALMutex); 1552 int res = update_delay(); 1553 // pthread_mutex_unlock(&mHALMutex); 1554 return res; 1555 } 1556 1557 int MPLSensor::update_delay() { 1558 VHANDLER_LOG; 1559 1560 int res = 0; 1561 int64_t got; 1562 1563 pthread_mutex_lock(&GlobalHalMutex); 1564 if (mEnabled) { 1565 int64_t wanted = 1000000000; 1566 int64_t wanted_3rd_party_sensor = 1000000000; 1567 1568 // Sequence to change sensor's FIFO rate 1569 // 1. enable Power state 1570 // 2. reset master enable 1571 // 3. Update delay 1572 // 4. set master enable 1573 1574 // ensure power on 1575 onPower(1); 1576 1577 // reset master enable 1578 masterEnable(0); 1579 1580 /* search the minimum delay requested across all enabled sensors */ 1581 for (int i = 0; i < numSensors; i++) { 1582 if (mEnabled & (1 << i)) { 1583 int64_t ns = mDelays[i]; 1584 wanted = wanted < ns ? wanted : ns; 1585 } 1586 } 1587 1588 // same delay for 3rd party Accel or Compass 1589 wanted_3rd_party_sensor = wanted; 1590 1591 /* mpl rate in us in future maybe different for 1592 gyro vs compass vs accel */ 1593 int rateInus = (int)wanted / 1000LL; 1594 int mplGyroRate = rateInus; 1595 int mplAccelRate = rateInus; 1596 int mplCompassRate = rateInus; 1597 1598 LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : " 1599 "%llu ns, mpl rate: %d us, (%.2f Hz)", 1600 wanted, rateInus, 1000000000.f / wanted); 1601 1602 /* set rate in MPL */ 1603 /* compass can only do 100Hz max */ 1604 inv_set_gyro_sample_rate(mplGyroRate); 1605 inv_set_accel_sample_rate(mplAccelRate); 1606 inv_set_compass_sample_rate(mplCompassRate); 1607 1608 /* TODO: Test 200Hz */ 1609 // inv_set_gyro_sample_rate(5000); 1610 LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate); 1611 LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate); 1612 LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate); 1613 1614 int enabled_sensors = mEnabled; 1615 int tempFd = -1; 1616 if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { 1617 if (isLowPowerQuatEnabled() || 1618 (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) { 1619 bool setDMPrate= 0; 1620 // Set LP Quaternion sample rate if enabled 1621 if (checkLPQuaternion()) { 1622 if (wanted < RATE_200HZ) { 1623 enableLPQuaternion(0); 1624 } else { 1625 inv_set_quat_sample_rate(rateInus); 1626 setDMPrate= 1; 1627 } 1628 } 1629 1630 if (checkDMPOrientation() || setDMPrate==1) { 1631 getDmpRate(&wanted); 1632 } 1633 } 1634 1635 int64_t tempRate = wanted; 1636 LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion"); 1637 //nsToHz 1638 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 1639 1000000000.f / tempRate, mpu.gyro_fifo_rate, 1640 getTimestamp()); 1641 tempFd = open(mpu.gyro_fifo_rate, O_RDWR); 1642 res = write_attribute_sensor(tempFd, 1000000000.f / tempRate); 1643 if(res < 0) { 1644 LOGE("HAL:GYRO update delay error"); 1645 } 1646 1647 //nsToHz (BMA250) 1648 if(USE_THIRD_PARTY_ACCEL) { 1649 LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)", 1650 wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate, 1651 getTimestamp()); 1652 tempFd = open(mpu.accel_fifo_rate, O_RDWR); 1653 res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L); 1654 LOGE_IF(res < 0, "HAL:ACCEL update delay error"); 1655 } 1656 1657 if (!mCompassSensor->isIntegrated()) { 1658 LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz", 1000000000.f / wanted_3rd_party_sensor); 1659 mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor); 1660 got = mCompassSensor->getDelay(ID_M); 1661 inv_set_compass_sample_rate(got / 1000); 1662 } 1663 1664 } else { 1665 1666 if (GY_ENABLED) { 1667 wanted = (mDelays[Gyro] <= mDelays[RawGyro]? 1668 (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]): 1669 (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro])); 1670 1671 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { 1672 getDmpRate(&wanted); 1673 } 1674 1675 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 1676 1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp()); 1677 tempFd = open(mpu.gyro_fifo_rate, O_RDWR); 1678 res = write_attribute_sensor(tempFd, 1000000000.f / wanted); 1679 LOGE_IF(res < 0, "HAL:GYRO update delay error"); 1680 } 1681 1682 if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */ 1683 if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) { 1684 wanted = mDelays[Gyro]; 1685 } 1686 else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) { 1687 wanted = mDelays[RawGyro]; 1688 1689 } else { 1690 wanted = mDelays[Accelerometer]; 1691 } 1692 1693 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { 1694 getDmpRate(&wanted); 1695 } 1696 1697 /* TODO: use function pointers to calculate delay value specific to vendor */ 1698 LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", 1699 1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp()); 1700 tempFd = open(mpu.accel_fifo_rate, O_RDWR); 1701 if(USE_THIRD_PARTY_ACCEL) { 1702 //BMA250 in ms 1703 res = write_attribute_sensor(tempFd, wanted / 1000000L); 1704 } 1705 else { 1706 //MPUxxxx in hz 1707 res = write_attribute_sensor(tempFd, 1000000000.f/wanted); 1708 } 1709 LOGE_IF(res < 0, "HAL:ACCEL update delay error"); 1710 } 1711 1712 /* Invensense compass calibration */ 1713 if (M_ENABLED) { 1714 if (!mCompassSensor->isIntegrated()) { 1715 wanted = mDelays[MagneticField]; 1716 } else { 1717 if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) { 1718 wanted = mDelays[Gyro]; 1719 } 1720 else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) { 1721 wanted = mDelays[RawGyro]; 1722 } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) { 1723 wanted = mDelays[Accelerometer]; 1724 } else { 1725 wanted = mDelays[MagneticField]; 1726 } 1727 1728 if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { 1729 getDmpRate(&wanted); 1730 } 1731 } 1732 1733 mCompassSensor->setDelay(ID_M, wanted); 1734 got = mCompassSensor->getDelay(ID_M); 1735 inv_set_compass_sample_rate(got / 1000); 1736 } 1737 1738 } 1739 1740 unsigned long sensors = mLocalSensorMask & mMasterSensorMask; 1741 if (sensors & 1742 (INV_THREE_AXIS_GYRO 1743 | INV_THREE_AXIS_ACCEL 1744 | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { 1745 res = masterEnable(1); 1746 } else { // all sensors idle -> reduce power 1747 res = onPower(0); 1748 } 1749 } 1750 pthread_mutex_unlock(&GlobalHalMutex); 1751 return res; 1752 } 1753 1754 /* For Third Party Accel Input Subsystem Drivers only */ 1755 /* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */ 1756 int MPLSensor::readAccelEvents(sensors_event_t* /*data*/, int count) 1757 { 1758 VHANDLER_LOG; 1759 1760 if (count < 1) 1761 return -EINVAL; 1762 1763 ssize_t n = mAccelInputReader.fill(accel_fd); 1764 if (n < 0) { 1765 LOGE("HAL:missed accel events, exit"); 1766 return n; 1767 } 1768 1769 int numEventReceived = 0; 1770 input_event const* event; 1771 int nb, done = 0; 1772 1773 while (!done && count && mAccelInputReader.readEvent(&event)) { 1774 int type = event->type; 1775 if (type == EV_ABS) { 1776 if (event->code == EVENT_TYPE_ACCEL_X) { 1777 mPendingMask |= 1 << Accelerometer; 1778 mCachedAccelData[0] = event->value; 1779 } else if (event->code == EVENT_TYPE_ACCEL_Y) { 1780 mPendingMask |= 1 << Accelerometer; 1781 mCachedAccelData[1] = event->value; 1782 } else if (event->code == EVENT_TYPE_ACCEL_Z) { 1783 mPendingMask |= 1 << Accelerometer; 1784 mCachedAccelData[2] =event-> value; 1785 } 1786 } else if (type == EV_SYN) { 1787 done = 1; 1788 if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { 1789 inv_build_accel(mCachedAccelData, 0, getTimestamp()); 1790 } 1791 } else { 1792 LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)", 1793 type, event->code); 1794 } 1795 mAccelInputReader.next(); 1796 } 1797 1798 return numEventReceived; 1799 } 1800 1801 /** 1802 * Should be called after reading at least one of gyro 1803 * compass or accel data. (Also okay for handling all of them). 1804 * @returns 0, if successful, error number if not. 1805 */ 1806 /* TODO: This should probably be called "int readEvents(...)" 1807 * and readEvents() called "void cacheData(void)". 1808 */ 1809 int MPLSensor::executeOnData(sensors_event_t* data, int count) 1810 { 1811 VFUNC_LOG; 1812 1813 inv_execute_on_data(); 1814 1815 int numEventReceived = 0; 1816 1817 long msg; 1818 msg = inv_get_message_level_0(1); 1819 if (msg) { 1820 if (msg & INV_MSG_MOTION_EVENT) { 1821 LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n"); 1822 } 1823 if (msg & INV_MSG_NO_MOTION_EVENT) { 1824 LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n"); 1825 /* after the first no motion, the gyro should be 1826 calibrated well */ 1827 mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH; 1828 /* if gyros are on and we got a no motion, set a flag 1829 indicating that the cal file can be written. */ 1830 mHaveGoodMpuCal = true; 1831 } 1832 } 1833 1834 // load up virtual sensors 1835 for (int i = 0; i < numSensors; i++) { 1836 int update; 1837 if (mEnabled & (1 << i)) { 1838 update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i); 1839 mPendingMask |= (1 << i); 1840 1841 if (update && (count > 0)) { 1842 *data++ = mPendingEvents[i]; 1843 count--; 1844 numEventReceived++; 1845 } 1846 } 1847 } 1848 1849 return numEventReceived; 1850 } 1851 1852 // collect data for MPL (but NOT sensor service currently), from driver layer 1853 /* TODO: FIX! data and count are not used, results is hardcoded to 0 */ 1854 /* TODO: This should probably be called "void cacheEvents(void)" 1855 * And executeOnData() should be int readEvents(data,count) 1856 */ 1857 int MPLSensor::readEvents(sensors_event_t* /*data*/, int /*count*/) { 1858 1859 1860 int lp_quaternion_on = 0, nbyte; 1861 int i, nb, mask = 0, numEventReceived = 0, 1862 sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) + 1863 ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) + 1864 (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1 : 0); 1865 char *rdata = mIIOBuffer; 1866 1867 nbyte= (8 * sensors + 8) * 1; 1868 1869 if (isLowPowerQuatEnabled()) { 1870 lp_quaternion_on = checkLPQuaternion(); 1871 if (lp_quaternion_on) { 1872 nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data 1873 } 1874 } 1875 1876 // pthread_mutex_lock(&mMplMutex); 1877 // pthread_mutex_lock(&mHALMutex); 1878 1879 ssize_t rsize = read(iio_fd, rdata, nbyte); 1880 if (sensors == 0) { 1881 // read(iio_fd, rdata, nbyte); 1882 rsize = read(iio_fd, rdata, sizeof(mIIOBuffer)); 1883 } 1884 1885 #ifdef TESTING 1886 LOGI("get one sample of IIO data with size: %d", rsize); 1887 LOGI("sensors: %d", sensors); 1888 1889 LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d", 1890 *((short *) (rdata + 0)), *((short *) (rdata + 2)), 1891 *((short *) (rdata + 4))); 1892 LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d", 1893 *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))), 1894 *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))), 1895 *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))); 1896 1897 LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS & 1898 mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d", 1899 *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + 1900 ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))), 1901 *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + 1902 ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))), 1903 *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + 1904 ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))); 1905 #endif 1906 1907 if (rsize < (nbyte - 8)) { 1908 LOGE("HAL:ERR Full data packet was not read. rsize=%zd nbyte=%d sensors=%d errno=%d(%s)", 1909 rsize, nbyte, sensors, errno, strerror(errno)); 1910 return -1; 1911 } 1912 1913 if (isLowPowerQuatEnabled() && lp_quaternion_on) { 1914 1915 for (i=0; i< 4; i++) { 1916 mCachedQuaternionData[i]= *(long*)rdata; 1917 rdata += sizeof(long); 1918 } 1919 } 1920 1921 for (i = 0; i < 3; i++) { 1922 if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { 1923 mCachedGyroData[i] = *((short *) (rdata + i * 2)); 1924 } 1925 if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { 1926 mCachedAccelData[i] = *((short *) (rdata + i * 2 + 1927 ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))); 1928 } 1929 if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) { 1930 mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1))); 1931 } 1932 } 1933 1934 mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) + 1935 ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0)); 1936 if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() && 1937 (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) { 1938 mask |= 1 << MagneticField; 1939 } 1940 1941 mSensorTimestamp = *((long long *) (rdata + 8 * sensors)); 1942 if (mCompassSensor->isIntegrated()) { 1943 mCompassTimestamp = mSensorTimestamp; 1944 } 1945 1946 if (mask & (1 << Gyro)) { 1947 // send down temperature every 0.5 seconds 1948 // with timestamp measured in "driver" layer 1949 if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) { 1950 mTempCurrentTime = mSensorTimestamp; 1951 long long temperature[2]; 1952 if(inv_read_temperature(temperature) == 0) { 1953 LOGV_IF(INPUT_DATA, 1954 "HAL:inv_read_temperature = %lld, timestamp= %lld", 1955 temperature[0], temperature[1]); 1956 inv_build_temp(temperature[0], temperature[1]); 1957 } 1958 #ifdef TESTING 1959 long bias[3], temp, temp_slope[3]; 1960 inv_get_gyro_bias(bias, &temp); 1961 inv_get_gyro_ts(temp_slope); 1962 1963 LOGI("T: %.3f " 1964 "GB: %+13f %+13f %+13f " 1965 "TS: %+13f %+13f %+13f " 1966 "\n", 1967 (float)temperature[0] / 65536.f, 1968 (float)bias[0] / 65536.f / 16.384f, 1969 (float)bias[1] / 65536.f / 16.384f, 1970 (float)bias[2] / 65536.f / 16.384f, 1971 temp_slope[0] / 65536.f, 1972 temp_slope[1] / 65536.f, 1973 temp_slope[2] / 65536.f); 1974 #endif 1975 } 1976 1977 mPendingMask |= 1 << Gyro; 1978 mPendingMask |= 1 << RawGyro; 1979 1980 if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { 1981 inv_build_gyro(mCachedGyroData, mSensorTimestamp); 1982 LOGV_IF(INPUT_DATA, 1983 "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld", 1984 mCachedGyroData[0], mCachedGyroData[1], 1985 mCachedGyroData[2], mSensorTimestamp); 1986 } 1987 } 1988 1989 if (mask & (1 << Accelerometer)) { 1990 mPendingMask |= 1 << Accelerometer; 1991 if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { 1992 inv_build_accel(mCachedAccelData, 0, mSensorTimestamp); 1993 LOGV_IF(INPUT_DATA, 1994 "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld", 1995 mCachedAccelData[0], mCachedAccelData[1], 1996 mCachedAccelData[2], mSensorTimestamp); 1997 } 1998 } 1999 2000 if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) { 2001 int status = 0; 2002 if (mCompassSensor->providesCalibration()) { 2003 status = mCompassSensor->getAccuracy(); 2004 status |= INV_CALIBRATED; 2005 } 2006 if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) { 2007 inv_build_compass(mCachedCompassData, status, 2008 mCompassTimestamp); 2009 LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", 2010 mCachedCompassData[0], mCachedCompassData[1], 2011 mCachedCompassData[2], mCompassTimestamp); 2012 } 2013 } 2014 2015 if (isLowPowerQuatEnabled() && lp_quaternion_on) { 2016 2017 inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp); 2018 LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld", 2019 mCachedQuaternionData[0], mCachedQuaternionData[1], 2020 mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp); 2021 } 2022 2023 // pthread_mutex_unlock(&mMplMutex); 2024 // pthread_mutex_unlock(&mHALMutex); 2025 2026 return numEventReceived; 2027 } 2028 2029 /* use for both MPUxxxx and third party compass */ 2030 int MPLSensor::readCompassEvents(sensors_event_t* /*data*/, int count) 2031 { 2032 VHANDLER_LOG; 2033 2034 if (count < 1) 2035 return -EINVAL; 2036 2037 int numEventReceived = 0; 2038 int done = 0; 2039 int nb; 2040 2041 // pthread_mutex_lock(&mMplMutex); 2042 // pthread_mutex_lock(&mHALMutex); 2043 2044 done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp); 2045 #ifdef COMPASS_YAS53x 2046 if (mCompassSensor->checkCoilsReset()) { 2047 //Reset relevant compass settings 2048 resetCompass(); 2049 } 2050 #endif 2051 if (done > 0) { 2052 int status = 0; 2053 if (mCompassSensor->providesCalibration()) { 2054 status = mCompassSensor->getAccuracy(); 2055 status |= INV_CALIBRATED; 2056 } 2057 if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) { 2058 inv_build_compass(mCachedCompassData, status, 2059 mCompassTimestamp); 2060 LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", 2061 mCachedCompassData[0], mCachedCompassData[1], 2062 mCachedCompassData[2], mCompassTimestamp); 2063 } 2064 } 2065 2066 // pthread_mutex_unlock(&mMplMutex); 2067 // pthread_mutex_unlock(&mHALMutex); 2068 2069 return numEventReceived; 2070 } 2071 2072 #ifdef COMPASS_YAS53x 2073 int MPLSensor::resetCompass() 2074 { 2075 VFUNC_LOG; 2076 2077 //Reset compass cal if enabled 2078 if (mFeatureActiveMask & INV_COMPASS_CAL) { 2079 LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal"); 2080 inv_init_vector_compass_cal(); 2081 } 2082 2083 //Reset compass fit if enabled 2084 if (mFeatureActiveMask & INV_COMPASS_FIT) { 2085 LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit"); 2086 inv_init_compass_fit(); 2087 } 2088 2089 return 0; 2090 } 2091 #endif 2092 2093 int MPLSensor::getFd() const 2094 { 2095 VFUNC_LOG; 2096 LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd); 2097 return iio_fd; 2098 } 2099 2100 int MPLSensor::getAccelFd() const 2101 { 2102 VFUNC_LOG; 2103 LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd); 2104 return accel_fd; 2105 } 2106 2107 int MPLSensor::getCompassFd() const 2108 { 2109 VFUNC_LOG; 2110 int fd = mCompassSensor->getFd(); 2111 LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd); 2112 return fd; 2113 } 2114 2115 int MPLSensor::turnOffAccelFifo() { 2116 int i, res, tempFd; 2117 char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable, 2118 mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable}; 2119 2120 for (i = 0; i < 3; i++) { 2121 res = write_sysfs_int(accel_fifo_enable[i], 0); 2122 if (res < 0) { 2123 return res; 2124 } 2125 } 2126 return 0; 2127 } 2128 2129 int MPLSensor::enableDmpOrientation(int en) 2130 { 2131 VFUNC_LOG; 2132 /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */ 2133 int res = 0; 2134 int enabled_sensors = mEnabled; 2135 2136 if (isMpu3050()) 2137 return res; 2138 2139 pthread_mutex_lock(&GlobalHalMutex); 2140 2141 // on power if not already On 2142 res = onPower(1); 2143 // reset master enable 2144 res = masterEnable(0); 2145 2146 if (en) { 2147 //Enable DMP orientation 2148 if (write_sysfs_int(mpu.display_orientation_on, en) < 0) { 2149 LOGE("HAL:ERR can't enable Android orientation"); 2150 res = -1; // indicate an err 2151 } 2152 2153 // open DMP Orient Fd 2154 res = openDmpOrientFd(); 2155 2156 // enable DMP 2157 res = onDMP(1); 2158 2159 // default DMP output rate to FIFO 2160 if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) { 2161 LOGE("HAL:ERR can't default DMP output rate"); 2162 } 2163 2164 // set DMP rate to 200Hz 2165 if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) { 2166 res = -1; 2167 LOGE("HAL:ERR can't set DMP rate to 200Hz"); 2168 } 2169 2170 // enable accel engine 2171 res = enableAccel(1); 2172 2173 // disable accel FIFO 2174 if (!A_ENABLED) { 2175 res = turnOffAccelFifo(); 2176 } 2177 2178 mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION; 2179 } else { 2180 // disable DMP 2181 res = onDMP(0); 2182 2183 // disable accel engine 2184 if (!A_ENABLED) { 2185 res = enableAccel(0); 2186 } 2187 } 2188 2189 res = masterEnable(1); 2190 pthread_mutex_unlock(&GlobalHalMutex); 2191 return res; 2192 } 2193 2194 int MPLSensor::openDmpOrientFd() 2195 { 2196 VFUNC_LOG; 2197 2198 if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) { 2199 LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened"); 2200 return -1; 2201 } 2202 2203 dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK); 2204 if (dmp_orient_fd < 0) { 2205 LOGE("HAL:ERR couldn't open dmpOrient node"); 2206 return -1; 2207 } else { 2208 LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_orient_fd opened : %d", dmp_orient_fd); 2209 return 0; 2210 } 2211 } 2212 2213 int MPLSensor::closeDmpOrientFd() 2214 { 2215 VFUNC_LOG; 2216 if (dmp_orient_fd >= 0) 2217 close(dmp_orient_fd); 2218 return 0; 2219 } 2220 2221 int MPLSensor::dmpOrientHandler(int orient) 2222 { 2223 VFUNC_LOG; 2224 2225 LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient); 2226 return 0; 2227 } 2228 2229 int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) { 2230 VFUNC_LOG; 2231 2232 char dummy[4]; 2233 int screen_orientation = 0; 2234 FILE *fp; 2235 2236 fp = fopen(mpu.event_display_orientation, "r"); 2237 if (fp == NULL) { 2238 LOGE("HAL:cannot open event_display_orientation"); 2239 return 0; 2240 } 2241 fscanf(fp, "%d\n", &screen_orientation); 2242 fclose(fp); 2243 2244 int numEventReceived = 0; 2245 2246 if (mDmpOrientationEnabled && count > 0) { 2247 sensors_event_t temp; 2248 2249 bzero(&temp, sizeof(temp)); /* Let's hope that SENSOR_TYPE_NONE is 0 */ 2250 temp.version = sizeof(sensors_event_t); 2251 temp.sensor = ID_SO; 2252 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION 2253 temp.type = SENSOR_TYPE_SCREEN_ORIENTATION; 2254 temp.screen_orientation = screen_orientation; 2255 #endif 2256 struct timespec ts; 2257 clock_gettime(CLOCK_MONOTONIC, &ts); 2258 temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; 2259 2260 *data++ = temp; 2261 count--; 2262 numEventReceived++; 2263 } 2264 2265 // read dummy data per driver's request 2266 dmpOrientHandler(screen_orientation); 2267 read(dmp_orient_fd, dummy, 4); 2268 2269 return numEventReceived; 2270 } 2271 2272 int MPLSensor::getDmpOrientFd() 2273 { 2274 VFUNC_LOG; 2275 2276 LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd); 2277 return dmp_orient_fd; 2278 2279 } 2280 2281 int MPLSensor::checkDMPOrientation() 2282 { 2283 VFUNC_LOG; 2284 return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0); 2285 } 2286 2287 int MPLSensor::getDmpRate(int64_t *wanted) 2288 { 2289 if (checkDMPOrientation() || checkLPQuaternion()) { 2290 // set DMP output rate to FIFO 2291 write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted); 2292 LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *wanted); 2293 2294 //DMP running rate must be @ 200Hz 2295 *wanted= RATE_200HZ; 2296 LOGV_IF(PROCESS_VERBOSE, "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted); 2297 } 2298 return 0; 2299 } 2300 2301 int MPLSensor::getPollTime() 2302 { 2303 VHANDLER_LOG; 2304 return mPollTime; 2305 } 2306 2307 bool MPLSensor::hasPendingEvents() const 2308 { 2309 VHANDLER_LOG; 2310 // if we are using the polling workaround, force the main 2311 // loop to check for data every time 2312 return (mPollTime != -1); 2313 } 2314 2315 /* TODO: support resume suspend when we gain more info about them*/ 2316 void MPLSensor::sleepEvent() 2317 { 2318 VFUNC_LOG; 2319 } 2320 2321 void MPLSensor::wakeEvent() 2322 { 2323 VFUNC_LOG; 2324 } 2325 2326 int MPLSensor::inv_float_to_q16(float *fdata, long *ldata) 2327 { 2328 VHANDLER_LOG; 2329 2330 if (!fdata || !ldata) 2331 return -1; 2332 ldata[0] = (long)(fdata[0] * 65536.f); 2333 ldata[1] = (long)(fdata[1] * 65536.f); 2334 ldata[2] = (long)(fdata[2] * 65536.f); 2335 return 0; 2336 } 2337 2338 int MPLSensor::inv_long_to_q16(long *fdata, long *ldata) 2339 { 2340 VHANDLER_LOG; 2341 2342 if (!fdata || !ldata) 2343 return -1; 2344 ldata[0] = (fdata[1] * 65536.f); 2345 ldata[1] = (fdata[2] * 65536.f); 2346 ldata[2] = (fdata[3] * 65536.f); 2347 return 0; 2348 } 2349 2350 int MPLSensor::inv_float_to_round(float *fdata, long *ldata) 2351 { 2352 VHANDLER_LOG; 2353 2354 if (!fdata || !ldata) 2355 return -1; 2356 ldata[0] = (long)fdata[0]; 2357 ldata[1] = (long)fdata[1]; 2358 ldata[2] = (long)fdata[2]; 2359 return 0; 2360 } 2361 2362 int MPLSensor::inv_float_to_round2(float *fdata, short *ldata) 2363 { 2364 VHANDLER_LOG; 2365 2366 if (!fdata || !ldata) 2367 return -1; 2368 ldata[0] = (short)fdata[0]; 2369 ldata[1] = (short)fdata[1]; 2370 ldata[2] = (short)fdata[2]; 2371 return 0; 2372 } 2373 2374 int MPLSensor::inv_read_temperature(long long *data) 2375 { 2376 VHANDLER_LOG; 2377 2378 int count = 0; 2379 char raw_buf[40]; 2380 long raw = 0; 2381 2382 long long timestamp = 0; 2383 2384 memset(raw_buf, 0, sizeof(raw_buf)); 2385 count = read_attribute_sensor(gyro_temperature_fd, raw_buf, 2386 sizeof(raw_buf)); 2387 if(count < 1) { 2388 LOGE("HAL:error reading gyro temperature"); 2389 return -1; 2390 } 2391 2392 count = sscanf(raw_buf, "%ld%lld", &raw, ×tamp); 2393 2394 if(count < 0) { 2395 return -1; 2396 } 2397 2398 LOGV_IF(ENG_VERBOSE, 2399 "HAL:temperature raw = %ld, timestamp = %lld, count = %d", 2400 raw, timestamp, count); 2401 data[0] = raw; 2402 data[1] = timestamp; 2403 2404 return 0; 2405 } 2406 2407 int MPLSensor::inv_read_dmp_state(int fd) 2408 { 2409 VFUNC_LOG; 2410 2411 if(fd < 0) 2412 return -1; 2413 2414 int count = 0; 2415 char raw_buf[10]; 2416 short raw = 0; 2417 2418 memset(raw_buf, 0, sizeof(raw_buf)); 2419 count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf)); 2420 if(count < 1) { 2421 LOGE("HAL:error reading dmp state"); 2422 close(fd); 2423 return -1; 2424 } 2425 count = sscanf(raw_buf, "%hd", &raw); 2426 if(count < 0) { 2427 LOGE("HAL:dmp state data is invalid"); 2428 close(fd); 2429 return -1; 2430 } 2431 LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count); 2432 close(fd); 2433 return (int)raw; 2434 } 2435 2436 int MPLSensor::inv_read_sensor_bias(int fd, long *data) 2437 { 2438 VFUNC_LOG; 2439 2440 if(fd == -1) { 2441 return -1; 2442 } 2443 2444 char buf[50]; 2445 char x[15], y[15], z[15]; 2446 2447 memset(buf, 0, sizeof(buf)); 2448 int count = read_attribute_sensor(fd, buf, sizeof(buf)); 2449 if(count < 1) { 2450 LOGE("HAL:Error reading gyro bias"); 2451 return -1; 2452 } 2453 count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z); 2454 if(count) { 2455 /* scale appropriately for MPL */ 2456 LOGV_IF(ENG_VERBOSE, 2457 "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)", 2458 atol(x), atol(y), atol(z)); 2459 2460 data[0] = (long)(atol(x) / 10000 * (1L << 16)); 2461 data[1] = (long)(atol(y) / 10000 * (1L << 16)); 2462 data[2] = (long)(atol(z) / 10000 * (1L << 16)); 2463 2464 LOGV_IF(ENG_VERBOSE, 2465 "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)", 2466 data[0], data[1], data[2]); 2467 } 2468 return 0; 2469 } 2470 2471 /** fill in the sensor list based on which sensors are configured. 2472 * return the number of configured sensors. 2473 * parameter list must point to a memory region of at least 7*sizeof(sensor_t) 2474 * parameter len gives the length of the buffer pointed to by list 2475 */ 2476 int MPLSensor::populateSensorList(struct sensor_t *list, int len) 2477 { 2478 VFUNC_LOG; 2479 2480 int numsensors; 2481 2482 if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) { 2483 LOGE("HAL:sensor list too small, not populating."); 2484 return -(sizeof(sSensorList) / sizeof(sensor_t)); 2485 } 2486 2487 /* fill in the base values */ 2488 memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t))); 2489 2490 /* first add gyro, accel and compass to the list */ 2491 2492 /* fill in gyro/accel values */ 2493 if(chip_ID == NULL) { 2494 LOGE("HAL:Can not get gyro/accel id"); 2495 } 2496 fillGyro(chip_ID, list); 2497 fillAccel(chip_ID, list); 2498 2499 // TODO: need fixes for unified HAL and 3rd-party solution 2500 mCompassSensor->fillList(&list[MagneticField]); 2501 2502 if(1) { 2503 numsensors = (sizeof(sSensorList) / sizeof(sensor_t)); 2504 /* all sensors will be added to the list 2505 fill in orientation values */ 2506 fillOrientation(list); 2507 /* fill in rotation vector values */ 2508 fillRV(list); 2509 /* fill in gravity values */ 2510 fillGravity(list); 2511 /* fill in Linear accel values */ 2512 fillLinearAccel(list); 2513 } else { 2514 /* no 9-axis sensors, zero fill that part of the list */ 2515 numsensors = 3; 2516 memset(list + 3, 0, 4 * sizeof(struct sensor_t)); 2517 } 2518 2519 return numsensors; 2520 } 2521 2522 void MPLSensor::fillAccel(const char* accel, struct sensor_t *list) 2523 { 2524 VFUNC_LOG; 2525 2526 if (accel) { 2527 if(accel != NULL && strcmp(accel, "BMA250") == 0) { 2528 list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; 2529 list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; 2530 list[Accelerometer].power = ACCEL_BMA250_POWER; 2531 list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; 2532 return; 2533 } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) { 2534 list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE; 2535 list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION; 2536 list[Accelerometer].power = ACCEL_MPU6050_POWER; 2537 list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY; 2538 return; 2539 } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) { 2540 list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE; 2541 list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION; 2542 list[Accelerometer].power = ACCEL_MPU6500_POWER; 2543 list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY; 2544 2545 return; 2546 } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) { 2547 list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE; 2548 list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION; 2549 list[Accelerometer].power = ACCEL_MPU9150_POWER; 2550 list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY; 2551 return; 2552 } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) { 2553 list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; 2554 list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; 2555 list[Accelerometer].power = ACCEL_BMA250_POWER; 2556 list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; 2557 return; 2558 } 2559 } 2560 2561 LOGE("HAL:unknown accel id %s -- " 2562 "params default to bma250 and might be wrong.", 2563 accel); 2564 list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; 2565 list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; 2566 list[Accelerometer].power = ACCEL_BMA250_POWER; 2567 list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; 2568 } 2569 2570 void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) 2571 { 2572 VFUNC_LOG; 2573 2574 if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) { 2575 list[Gyro].maxRange = GYRO_MPU3050_RANGE; 2576 list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; 2577 list[Gyro].power = GYRO_MPU3050_POWER; 2578 list[Gyro].minDelay = GYRO_MPU3050_MINDELAY; 2579 } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) { 2580 list[Gyro].maxRange = GYRO_MPU6050_RANGE; 2581 list[Gyro].resolution = GYRO_MPU6050_RESOLUTION; 2582 list[Gyro].power = GYRO_MPU6050_POWER; 2583 list[Gyro].minDelay = GYRO_MPU6050_MINDELAY; 2584 } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) { 2585 list[Gyro].maxRange = GYRO_MPU6500_RANGE; 2586 list[Gyro].resolution = GYRO_MPU6500_RESOLUTION; 2587 list[Gyro].power = GYRO_MPU6500_POWER; 2588 list[Gyro].minDelay = GYRO_MPU6500_MINDELAY; 2589 } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) { 2590 list[Gyro].maxRange = GYRO_MPU9150_RANGE; 2591 list[Gyro].resolution = GYRO_MPU9150_RESOLUTION; 2592 list[Gyro].power = GYRO_MPU9150_POWER; 2593 list[Gyro].minDelay = GYRO_MPU9150_MINDELAY; 2594 } else { 2595 LOGE("HAL:unknown gyro id -- gyro params will be wrong."); 2596 LOGE("HAL:default to use mpu3050 params"); 2597 list[Gyro].maxRange = GYRO_MPU3050_RANGE; 2598 list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; 2599 list[Gyro].power = GYRO_MPU3050_POWER; 2600 list[Gyro].minDelay = GYRO_MPU3050_MINDELAY; 2601 } 2602 2603 list[RawGyro].maxRange = list[Gyro].maxRange; 2604 list[RawGyro].resolution = list[Gyro].resolution; 2605 list[RawGyro].power = list[Gyro].power; 2606 list[RawGyro].minDelay = list[Gyro].minDelay; 2607 2608 return; 2609 } 2610 2611 /* fillRV depends on values of accel and compass in the list */ 2612 void MPLSensor::fillRV(struct sensor_t *list) 2613 { 2614 VFUNC_LOG; 2615 2616 /* compute power on the fly */ 2617 list[RotationVector].power = list[Gyro].power + 2618 list[Accelerometer].power + 2619 list[MagneticField].power; 2620 list[RotationVector].resolution = .00001; 2621 list[RotationVector].maxRange = 1.0; 2622 list[RotationVector].minDelay = 5000; 2623 2624 return; 2625 } 2626 2627 void MPLSensor::fillOrientation(struct sensor_t *list) 2628 { 2629 VFUNC_LOG; 2630 2631 list[Orientation].power = list[Gyro].power + 2632 list[Accelerometer].power + 2633 list[MagneticField].power; 2634 list[Orientation].resolution = .00001; 2635 list[Orientation].maxRange = 360.0; 2636 list[Orientation].minDelay = 5000; 2637 2638 return; 2639 } 2640 2641 void MPLSensor::fillGravity( struct sensor_t *list) 2642 { 2643 VFUNC_LOG; 2644 2645 list[Gravity].power = list[Gyro].power + 2646 list[Accelerometer].power + 2647 list[MagneticField].power; 2648 list[Gravity].resolution = .00001; 2649 list[Gravity].maxRange = 9.81; 2650 list[Gravity].minDelay = 5000; 2651 2652 return; 2653 } 2654 2655 void MPLSensor::fillLinearAccel(struct sensor_t *list) 2656 { 2657 VFUNC_LOG; 2658 2659 list[LinearAccel].power = list[Gyro].power + 2660 list[Accelerometer].power + 2661 list[MagneticField].power; 2662 list[LinearAccel].resolution = list[Accelerometer].resolution; 2663 list[LinearAccel].maxRange = list[Accelerometer].maxRange; 2664 list[LinearAccel].minDelay = 5000; 2665 2666 return; 2667 } 2668 2669 int MPLSensor::inv_init_sysfs_attributes(void) 2670 { 2671 VFUNC_LOG; 2672 2673 unsigned char i; 2674 char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN]; 2675 char *sptr; 2676 char **dptr; 2677 int num; 2678 2679 sysfs_names_ptr = 2680 (char*)calloc(1, sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); 2681 sptr = sysfs_names_ptr; 2682 if (sptr == NULL) { 2683 LOGE("HAL:couldn't alloc mem for sysfs paths"); 2684 return -1; 2685 } 2686 2687 dptr = (char**)&mpu; 2688 i = 0; 2689 do { 2690 *dptr++ = sptr; 2691 sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); 2692 } while (++i < MAX_SYSFS_ATTRB); 2693 2694 // get proper (in absolute/relative) IIO path & build MPU's sysfs paths 2695 // inv_get_sysfs_abs_path(sysfs_path); 2696 if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) { 2697 ALOGE("MPLSensor failed get sysfs path"); 2698 return -1; 2699 } 2700 2701 if(INV_SUCCESS != inv_get_iio_trigger_path(iio_trigger_path)) { 2702 ALOGE("MPLSensor failed get iio trigger path"); 2703 return -1; 2704 } 2705 2706 sprintf(mpu.key, "%s%s", sysfs_path, "/key"); 2707 sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable"); 2708 sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length"); 2709 sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state"); 2710 sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en"); 2711 sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name"); 2712 sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger"); 2713 2714 sprintf(mpu.dmp_firmware, "%s%s", sysfs_path,"/dmp_firmware"); 2715 sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded"); 2716 sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on"); 2717 sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on"); 2718 sprintf(mpu.dmp_event_int_on,"%s%s", sysfs_path, "/dmp_event_int_on"); 2719 sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate"); 2720 sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on"); 2721 2722 // TODO: for self test 2723 sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test"); 2724 2725 sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature"); 2726 sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable"); 2727 sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); 2728 sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix"); 2729 sprintf(mpu.gyro_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en"); 2730 sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en"); 2731 sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en"); 2732 2733 sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable"); 2734 sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); 2735 sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix"); 2736 2737 2738 #ifndef THIRD_PARTY_ACCEL //MPUxxxx 2739 sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale"); 2740 // TODO: for bias settings 2741 sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias"); 2742 #endif 2743 2744 sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en"); 2745 sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en"); 2746 sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en"); 2747 2748 sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on"); 2749 sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en"); 2750 sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en"); 2751 sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en"); 2752 sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en"); 2753 2754 sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on"); 2755 sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation"); 2756 2757 #if SYSFS_VERBOSE 2758 // test print sysfs paths 2759 dptr = (char**)&mpu; 2760 for (i = 0; i < MAX_SYSFS_ATTRB; i++) { 2761 LOGE("HAL:sysfs path: %s", *dptr++); 2762 } 2763 #endif 2764 return 0; 2765 } 2766 2767 /* TODO: stop manually testing/using 0 and 1 instead of 2768 * false and true, but just use 0 and non-0. 2769 * This allows passing 0 and non-0 ints around instead of 2770 * having to convert to 1 and test against 1. 2771 */ 2772 bool MPLSensor::isMpu3050() 2773 { 2774 return !strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050"); 2775 } 2776 2777 int MPLSensor::isLowPowerQuatEnabled() 2778 { 2779 #ifdef ENABLE_LP_QUAT_FEAT 2780 return !isMpu3050(); 2781 #else 2782 return 0; 2783 #endif 2784 } 2785 2786 int MPLSensor::isDmpDisplayOrientationOn() 2787 { 2788 #ifdef ENABLE_DMP_DISPL_ORIENT_FEAT 2789 return !isMpu3050(); 2790 #else 2791 return 0; 2792 #endif 2793 } 2794
