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