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