1 /* 2 * Copyright (C) 2014 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 #ifndef ANDROID_MPL_SENSOR_H 18 #define ANDROID_MPL_SENSOR_H 19 20 #include <stdint.h> 21 #include <errno.h> 22 #include <sys/cdefs.h> 23 #include <sys/types.h> 24 #include <poll.h> 25 #include <time.h> 26 #include <utils/Vector.h> 27 #include <utils/KeyedVector.h> 28 #include <utils/String8.h> 29 #include "sensors.h" 30 #include "SensorBase.h" 31 #include "InputEventReader.h" 32 33 #ifndef INVENSENSE_COMPASS_CAL 34 #pragma message("unified HAL for AKM") 35 #include "CompassSensor.AKM.h" 36 #endif 37 38 #ifdef SENSOR_ON_PRIMARY_BUS 39 #pragma message("Sensor on Primary Bus") 40 #include "CompassSensor.IIO.primary.h" 41 #else 42 #pragma message("Sensor on Secondary Bus") 43 #include "CompassSensor.IIO.9150.h" 44 #endif 45 46 class PressureSensor; 47 48 /*****************************************************************************/ 49 /* Sensors Enable/Disable Mask 50 *****************************************************************************/ 51 #define MAX_CHIP_ID_LEN (20) 52 53 #define INV_THREE_AXIS_GYRO (0x000F) 54 #define INV_THREE_AXIS_ACCEL (0x0070) 55 #define INV_THREE_AXIS_COMPASS (0x0380) 56 #define INV_ONE_AXIS_PRESSURE (0x0400) 57 #define INV_ALL_SENSORS (0x7FFF) 58 59 #ifdef INVENSENSE_COMPASS_CAL 60 #define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \ 61 | INV_THREE_AXIS_COMPASS \ 62 | INV_THREE_AXIS_GYRO) 63 #else 64 #define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \ 65 | INV_THREE_AXIS_COMPASS \ 66 | INV_THREE_AXIS_GYRO) 67 #endif 68 69 // mask of virtual sensors that require gyro + accel + compass data 70 #define VIRTUAL_SENSOR_9AXES_MASK ( \ 71 (1 << Orientation) \ 72 | (1 << RotationVector) \ 73 | (1 << LinearAccel) \ 74 | (1 << Gravity) \ 75 ) 76 // mask of virtual sensors that require gyro + accel data (but no compass data) 77 #define VIRTUAL_SENSOR_GYRO_6AXES_MASK ( \ 78 (1 << GameRotationVector) \ 79 ) 80 // mask of virtual sensors that require mag + accel data (but no gyro data) 81 #define VIRTUAL_SENSOR_MAG_6AXES_MASK ( \ 82 (1 << GeomagneticRotationVector) \ 83 ) 84 // mask of all virtual sensors 85 #define VIRTUAL_SENSOR_ALL_MASK ( \ 86 VIRTUAL_SENSOR_9AXES_MASK \ 87 | VIRTUAL_SENSOR_GYRO_6AXES_MASK \ 88 | VIRTUAL_SENSOR_MAG_6AXES_MASK \ 89 ) 90 91 // bit mask of current MPL active features (mMplFeatureActiveMask) 92 #define INV_COMPASS_CAL 0x01 93 #define INV_COMPASS_FIT 0x02 94 95 // bit mask of current DMP active features (mFeatureActiveMask) 96 #define INV_DMP_QUATERNION 0x001 //3 elements without real part, 32 bit each 97 #define INV_DMP_DISPL_ORIENTATION 0x002 //screen orientation 98 #define INV_DMP_SIGNIFICANT_MOTION 0x004 //significant motion 99 #define INV_DMP_PEDOMETER 0x008 //interrupt-based pedometer 100 #define INV_DMP_PEDOMETER_STEP 0x010 //timer-based pedometer 101 #define INV_DMP_PED_STANDALONE 0x020 //timestamps only 102 #define INV_DMP_6AXIS_QUATERNION 0x040 //3 elements without real part, 32 bit each 103 #define INV_DMP_PED_QUATERNION 0x080 //3 elements without real part, 16 bit each 104 #define INV_DMP_PED_INDICATOR 0x100 //tag along header with step indciator 105 #define INV_DMP_BATCH_MODE 0x200 //batch mode 106 107 // bit mask of whether DMP should be turned on 108 #define DMP_FEATURE_MASK ( \ 109 (INV_DMP_QUATERNION) \ 110 | (INV_DMP_DISPL_ORIENTATION) \ 111 | (INV_DMP_SIGNIFICANT_MOTION) \ 112 | (INV_DMP_PEDOMETER) \ 113 | (INV_DMP_PEDOMETER_STEP) \ 114 | (INV_DMP_6AXIS_QUATERNION) \ 115 | (INV_DMP_PED_QUATERNION) \ 116 | (INV_DMP_BATCH_MODE) \ 117 ) 118 119 // bit mask of DMP features as sensors 120 #define DMP_SENSOR_MASK ( \ 121 (INV_DMP_DISPL_ORIENTATION) \ 122 | (INV_DMP_SIGNIFICANT_MOTION) \ 123 | (INV_DMP_PEDOMETER) \ 124 | (INV_DMP_PEDOMETER_STEP) \ 125 | (INV_DMP_6AXIS_QUATERNION) \ 126 ) 127 128 // data header format used by kernel driver. 129 #define DATA_FORMAT_STEP 0x0001 130 #define DATA_FORMAT_MARKER 0x0010 131 #define DATA_FORMAT_EMPTY_MARKER 0x0020 132 #define DATA_FORMAT_PED_STANDALONE 0x0100 133 #define DATA_FORMAT_PED_QUAT 0x0200 134 #define DATA_FORMAT_6_AXIS 0x0400 135 #define DATA_FORMAT_QUAT 0x0800 136 #define DATA_FORMAT_COMPASS 0x1000 137 #define DATA_FORMAT_COMPASS_OF 0x1800 138 #define DATA_FORMAT_GYRO 0x2000 139 #define DATA_FORMAT_ACCEL 0x4000 140 #define DATA_FORMAT_PRESSURE 0x8000 141 #define DATA_FORMAT_MASK 0xffff 142 143 #define BYTES_PER_SENSOR 8 144 #define BYTES_PER_SENSOR_PACKET 16 145 #define QUAT_ONLY_LAST_PACKET_OFFSET 16 146 #define BYTES_QUAT_DATA 24 147 #define MAX_READ_SIZE BYTES_QUAT_DATA 148 #define MAX_SUSPEND_BATCH_PACKET_SIZE 1024 149 #define MAX_PACKET_SIZE 80 //8 * 4 + (2 * 24) 150 151 /* Uncomment to enable Low Power Quaternion */ 152 #define ENABLE_LP_QUAT_FEAT 153 154 /* Enable Pressure sensor support */ 155 #undef ENABLE_PRESSURE 156 157 /* Screen Orientation is not currently supported */ 158 int isDmpScreenAutoRotationEnabled() 159 { 160 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION 161 return 1; 162 #else 163 return 0; 164 #endif 165 } 166 167 int (*m_pt2AccelCalLoadFunc)(long *bias) = NULL; 168 /*****************************************************************************/ 169 /** MPLSensor implementation which fits into the HAL example for crespo provided 170 * by Google. 171 * WARNING: there may only be one instance of MPLSensor, ever. 172 */ 173 174 class MPLSensor: public SensorBase 175 { 176 typedef int (MPLSensor::*hfunc_t)(sensors_event_t*); 177 178 public: 179 180 MPLSensor(CompassSensor *, int (*m_pt2AccelCalLoadFunc)(long*) = 0); 181 virtual ~MPLSensor(); 182 183 virtual int setDelay(int32_t handle, int64_t ns); 184 virtual int enable(int32_t handle, int enabled); 185 virtual int batch(int handle, int flags, int64_t period_ns, int64_t timeout); 186 virtual int flush(int handle); 187 int selectAndSetQuaternion(int batchMode, int mEnabled, long long featureMask); 188 int checkBatchEnabled(); 189 int setBatch(int en, int toggleEnable); 190 int writeBatchTimeout(int en); 191 int writeBatchTimeout(int en, int64_t timeoutInMs); 192 int32_t getEnableMask() { return mEnabled; } 193 void getHandle(int32_t handle, int &what, android::String8 &sname); 194 195 virtual int readEvents(sensors_event_t *data, int count); 196 virtual int getFd() const; 197 virtual int getAccelFd() const; 198 virtual int getCompassFd() const; 199 virtual int getPollTime(); 200 virtual int getStepCountPollTime(); 201 virtual bool hasPendingEvents() const; 202 virtual bool hasStepCountPendingEvents(); 203 int populateSensorList(struct sensor_t *list, int len); 204 205 void buildCompassEvent(); 206 void buildMpuEvent(); 207 int checkValidHeader(unsigned short data_format); 208 209 int turnOffAccelFifo(); 210 int turnOffGyroFifo(); 211 int enableDmpOrientation(int); 212 int dmpOrientHandler(int); 213 int readDmpOrientEvents(sensors_event_t* data, int count); 214 int getDmpOrientFd(); 215 int openDmpOrientFd(); 216 int closeDmpOrientFd(); 217 218 int getDmpRate(int64_t *); 219 int checkDMPOrientation(); 220 221 int getDmpSignificantMotionFd(); 222 int readDmpSignificantMotionEvents(sensors_event_t* data, int count); 223 int enableDmpSignificantMotion(int); 224 int significantMotionHandler(sensors_event_t* data); 225 bool checkSmdSupport(){return (mDmpSignificantMotionEnabled);}; 226 227 int enableDmpPedometer(int, int); 228 int readDmpPedometerEvents(sensors_event_t* data, int count, int32_t id, int outputType); 229 int getDmpPedometerFd(); 230 bool checkPedometerSupport() {return (mDmpPedometerEnabled || mDmpStepCountEnabled);}; 231 bool checkOrientationSupport() {return ((isDmpDisplayOrientationOn() 232 && (mDmpOrientationEnabled 233 || !isDmpScreenAutoRotationEnabled())));}; 234 235 protected: 236 CompassSensor *mCompassSensor; 237 PressureSensor *mPressureSensor; 238 239 int gyroHandler(sensors_event_t *data); 240 int rawGyroHandler(sensors_event_t *data); 241 int accelHandler(sensors_event_t *data); 242 int compassHandler(sensors_event_t *data); 243 int rawCompassHandler(sensors_event_t *data); 244 int rvHandler(sensors_event_t *data); 245 int grvHandler(sensors_event_t *data); 246 int laHandler(sensors_event_t *data); 247 int gravHandler(sensors_event_t *data); 248 int orienHandler(sensors_event_t *data); 249 int smHandler(sensors_event_t *data); 250 int pHandler(sensors_event_t *data); 251 int gmHandler(sensors_event_t *data); 252 int psHandler(sensors_event_t *data); 253 int sdHandler(sensors_event_t *data); 254 int scHandler(sensors_event_t *data); 255 int metaHandler(sensors_event_t *data, int flags); 256 void calcOrientationSensor(float *Rx, float *Val); 257 virtual int update_delay(); 258 259 void inv_set_device_properties(); 260 int inv_constructor_init(); 261 int inv_constructor_default_enable(); 262 int setAccelInitialState(); 263 int masterEnable(int en); 264 int enablePedStandalone(int en); 265 int enablePedStandaloneData(int en); 266 int enablePedQuaternion(int); 267 int enablePedQuaternionData(int); 268 int setPedQuaternionRate(int64_t wanted); 269 int enable6AxisQuaternion(int); 270 int enable6AxisQuaternionData(int); 271 int set6AxisQuaternionRate(int64_t wanted); 272 int enableLPQuaternion(int); 273 int enableQuaternionData(int); 274 int setQuaternionRate(int64_t wanted); 275 int enableAccelPedometer(int); 276 int enableAccelPedData(int); 277 int onDmp(int); 278 int enableGyro(int en); 279 int enableLowPowerAccel(int en); 280 int enableAccel(int en); 281 int enableCompass(int en, int rawSensorOn); 282 int enablePressure(int en); 283 int enableBatch(int64_t timeout); 284 void computeLocalSensorMask(int enabled_sensors); 285 int computeBatchSensorMask(int enableSensor, int checkNewBatchSensor); 286 int computeBatchDataOutput(); 287 int enableSensors(unsigned long sensors, int en, uint32_t changed); 288 int inv_read_temperature(long long *data); 289 int inv_read_dmp_state(int fd); 290 int inv_read_sensor_bias(int fd, long *data); 291 void inv_get_sensors_orientation(void); 292 int inv_init_sysfs_attributes(void); 293 int resetCompass(void); 294 void setCompassDelay(int64_t ns); 295 void enable_iio_sysfs(void); 296 int setDmpFeature(int en); 297 int computeAndSetDmpState(void); 298 int computeDmpState(bool* dmp_state); 299 int SetDmpState(bool dmpState); 300 int enablePedometer(int); 301 int enablePedIndicator(int en); 302 int checkPedStandaloneBatched(void); 303 int checkPedStandaloneEnabled(void); 304 int checkPedQuatEnabled(); 305 int check6AxisQuatEnabled(); 306 int checkLPQRateSupported(); 307 int checkLPQuaternion(); 308 int checkAccelPed(); 309 void setInitial6QuatValue(); 310 int writeSignificantMotionParams(bool toggleEnable, 311 uint32_t delayThreshold1, uint32_t delayThreshold2, 312 uint32_t motionThreshold); 313 long mMasterSensorMask; 314 long mLocalSensorMask; 315 int mPollTime; 316 int64_t mStepCountPollTime; 317 bool mHaveGoodMpuCal; // flag indicating that the cal file can be written 318 int mGyroAccuracy; // value indicating the quality of the gyro calibr. 319 int mAccelAccuracy; // value indicating the quality of the accel calibr. 320 int mCompassAccuracy; // value indicating the quality of the compass calibr. 321 struct pollfd mPollFds[5]; 322 pthread_mutex_t mMplMutex; 323 pthread_mutex_t mHALMutex; 324 325 char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH]; 326 327 int iio_fd; 328 int accel_fd; 329 int mpufifo_fd; 330 int gyro_temperature_fd; 331 int accel_x_offset_fd; 332 int accel_y_offset_fd; 333 int accel_z_offset_fd; 334 335 int accel_x_dmp_bias_fd; 336 int accel_y_dmp_bias_fd; 337 int accel_z_dmp_bias_fd; 338 339 int gyro_x_offset_fd; 340 int gyro_y_offset_fd; 341 int gyro_z_offset_fd; 342 343 int gyro_x_dmp_bias_fd; 344 int gyro_y_dmp_bias_fd; 345 int gyro_z_dmp_bias_fd; 346 347 int dmp_orient_fd; 348 int mDmpOrientationEnabled; 349 350 int dmp_sign_motion_fd; 351 int mDmpSignificantMotionEnabled; 352 353 int dmp_pedometer_fd; 354 int mDmpPedometerEnabled; 355 int mDmpStepCountEnabled; 356 357 uint32_t mEnabled; 358 uint32_t mBatchEnabled; 359 android::Vector<int> mFlushSensorEnabledVector; 360 uint32_t mOldBatchEnabledMask; 361 int64_t mBatchTimeoutInMs; 362 sensors_event_t mPendingEvents[NumSensors]; 363 sensors_event_t mPendingFlushEvents[NumSensors]; 364 sensors_event_t mSmEvents; 365 sensors_event_t mSdEvents; 366 sensors_event_t mScEvents; 367 int64_t mDelays[NumSensors]; 368 int64_t mBatchDelays[NumSensors]; 369 int64_t mBatchTimeouts[NumSensors]; 370 hfunc_t mHandlers[NumSensors]; 371 short mCachedGyroData[3]; 372 long mCachedAccelData[3]; 373 long mCachedCompassData[3]; 374 long mCachedQuaternionData[3]; 375 long mCached6AxisQuaternionData[3]; 376 long mCachedPedQuaternionData[3]; 377 long mCachedPressureData; 378 android::KeyedVector<int, int> mIrqFds; 379 380 InputEventCircularReader mAccelInputReader; 381 InputEventCircularReader mGyroInputReader; 382 383 int mCompassOverFlow; 384 bool mFirstRead; 385 short mTempScale; 386 short mTempOffset; 387 int64_t mTempCurrentTime; 388 int mAccelScale; 389 long mAccelSelfTestScale; 390 long mGyroScale; 391 long mGyroSelfTestScale; 392 long mCompassScale; 393 float mCompassBias[3]; 394 bool mFactoryGyroBiasAvailable; 395 long mFactoryGyroBias[3]; 396 bool mGyroBiasAvailable; 397 bool mGyroBiasApplied; 398 float mGyroBias[3]; //in body frame 399 long mGyroChipBias[3]; //in chip frame 400 bool mFactoryAccelBiasAvailable; 401 long mFactoryAccelBias[3]; 402 bool mAccelBiasAvailable; 403 bool mAccelBiasApplied; 404 long mAccelBias[3]; //in chip frame 405 406 uint32_t mPendingMask; 407 unsigned long mSensorMask; 408 409 char chip_ID[MAX_CHIP_ID_LEN]; 410 char mSysfsPath[MAX_SYSFS_NAME_LEN]; 411 412 signed char mGyroOrientation[9]; 413 signed char mAccelOrientation[9]; 414 415 int64_t mSensorTimestamp; 416 int64_t mCompassTimestamp; 417 int64_t mPressureTimestamp; 418 419 int64_t mGyroBatchRate; 420 int64_t mAccelBatchRate; 421 int64_t mCompassBatchRate; 422 int64_t mPressureBatchRate; 423 int64_t mQuatBatchRate; 424 int64_t mGyroRate; 425 int64_t mAccelRate; 426 int64_t mCompassRate; 427 int64_t mPressureRate; 428 int64_t mQuatRate; 429 int64_t mResetRate; 430 431 uint32_t mDataInterrupt; 432 433 bool mFirstBatchCall; 434 bool mEnableCalled; 435 436 struct sysfs_attrbs { 437 char *chip_enable; 438 char *power_state; 439 char *master_enable; 440 char *dmp_firmware; 441 char *firmware_loaded; 442 char *dmp_on; 443 char *dmp_int_on; 444 char *dmp_event_int_on; 445 char *tap_on; 446 char *key; 447 char *self_test; 448 char *temperature; 449 450 char *gyro_enable; 451 char *gyro_fifo_rate; 452 char *gyro_fsr; 453 char *gyro_orient; 454 char *gyro_fifo_enable; 455 char *gyro_rate; 456 457 char *accel_enable; 458 char *accel_fifo_rate; 459 char *accel_fsr; 460 char *accel_bias; 461 char *accel_orient; 462 char *accel_fifo_enable; 463 char *accel_rate; 464 465 char *three_axis_q_on; //formerly quaternion_on 466 char *three_axis_q_rate; 467 468 char *six_axis_q_on; 469 char *six_axis_q_rate; 470 471 char *six_axis_q_value; 472 473 char *ped_q_on; 474 char *ped_q_rate; 475 476 char *step_detector_on; 477 char *step_indicator_on; 478 479 char *in_timestamp_en; 480 char *in_timestamp_index; 481 char *in_timestamp_type; 482 483 char *buffer_length; 484 485 char *display_orientation_on; 486 char *event_display_orientation; 487 488 char *in_accel_x_offset; 489 char *in_accel_y_offset; 490 char *in_accel_z_offset; 491 char *in_accel_self_test_scale; 492 493 char *in_accel_x_dmp_bias; 494 char *in_accel_y_dmp_bias; 495 char *in_accel_z_dmp_bias; 496 497 char *in_gyro_x_offset; 498 char *in_gyro_y_offset; 499 char *in_gyro_z_offset; 500 char *in_gyro_self_test_scale; 501 502 char *in_gyro_x_dmp_bias; 503 char *in_gyro_y_dmp_bias; 504 char *in_gyro_z_dmp_bias; 505 506 char *event_smd; 507 char *smd_enable; 508 char *smd_delay_threshold; 509 char *smd_delay_threshold2; 510 char *smd_threshold; 511 char *batchmode_timeout; 512 char *batchmode_wake_fifo_full_on; 513 char *flush_batch; 514 515 char *pedometer_on; 516 char *pedometer_int_on; 517 char *event_pedometer; 518 char *pedometer_steps; 519 char *pedometer_step_thresh; 520 char *pedometer_counter; 521 522 char *motion_lpa_on; 523 } mpu; 524 525 char *sysfs_names_ptr; 526 int mMplFeatureActiveMask; 527 uint64_t mFeatureActiveMask; 528 bool mDmpOn; 529 int mPedUpdate; 530 int mPressureUpdate; 531 int64_t mQuatSensorTimestamp; 532 int64_t mStepSensorTimestamp; 533 uint64_t mLastStepCount; 534 int mLeftOverBufferSize; 535 char mLeftOverBuffer[1024]; 536 bool mInitial6QuatValueAvailable; 537 long mInitial6QuatValue[4]; 538 bool mFlushBatchSet; 539 uint32_t mSkipReadEvents; 540 bool mDataMarkerDetected; 541 bool mEmptyDataMarkerDetected; 542 int mDmpState; 543 544 private: 545 /* added for dynamic get sensor list */ 546 void fillAccel(const char* accel, struct sensor_t *list); 547 void fillGyro(const char* gyro, struct sensor_t *list); 548 void fillRV(struct sensor_t *list); 549 void fillGMRV(struct sensor_t *list); 550 void fillGRV(struct sensor_t *list); 551 void fillOrientation(struct sensor_t *list); 552 void fillGravity(struct sensor_t *list); 553 void fillLinearAccel(struct sensor_t *list); 554 void fillSignificantMotion(struct sensor_t *list); 555 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION 556 void fillScreenOrientation(struct sensor_t *list); 557 #endif 558 void storeCalibration(); 559 void loadDMP(); 560 bool isMpuNonDmp(); 561 int isLowPowerQuatEnabled(); 562 int isDmpDisplayOrientationOn(); 563 void getCompassBias(); 564 void getFactoryGyroBias(); 565 void setFactoryGyroBias(); 566 void getGyroBias(); 567 void setGyroZeroBias(); 568 void setGyroBias(); 569 void getFactoryAccelBias(); 570 void setFactoryAccelBias(); 571 void getAccelBias(); 572 void setAccelBias(); 573 int isCompassDisabled(); 574 int setBatchDataRates(); 575 int calcBatchDataRates(int64_t *gyro_rate, int64_t *accel_rate, int64_t *compass_rate, int64_t *pressure_rate, int64_t *quat_rate); 576 int setBatchDataRates(int64_t gyroRate, int64_t accelRate, int64_t compassRate, int64_t pressureRate, int64_t quatRate); 577 int resetDataRates(); 578 int calctDataRates(int64_t *resetRate, int64_t *gyroRate, int64_t *accelRate, int64_t *compassRate, int64_t *pressureRate); 579 int resetDataRates(int64_t resetRate, int64_t gyroRate, int64_t accelRate, int64_t compassRate, int64_t pressureRate); 580 void initBias(); 581 void resetMplStates(); 582 void sys_dump(bool fileMode); 583 int calcBatchTimeout(int en, int64_t *out); 584 }; 585 586 extern "C" { 587 void setCallbackObject(MPLSensor*); 588 MPLSensor *getCallbackObject(); 589 } 590 591 #endif // ANDROID_MPL_SENSOR_H 592
