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