/* * Copyright (C) 2014 Invensense, Inc. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef ANDROID_MPL_SENSOR_H #define ANDROID_MPL_SENSOR_H #include #include #include #include #include #include #include #include #include #include "sensors.h" #include "SensorBase.h" #include "InputEventReader.h" #ifndef INVENSENSE_COMPASS_CAL #pragma message("unified HAL for AKM") #include "CompassSensor.AKM.h" #endif #ifdef SENSOR_ON_PRIMARY_BUS #pragma message("Sensor on Primary Bus") #include "CompassSensor.IIO.primary.h" #else #pragma message("Sensor on Secondary Bus") #include "CompassSensor.IIO.9150.h" #endif class PressureSensor; /*****************************************************************************/ /* Sensors Enable/Disable Mask *****************************************************************************/ #define MAX_CHIP_ID_LEN (20) #define INV_THREE_AXIS_GYRO (0x000F) #define INV_THREE_AXIS_ACCEL (0x0070) #define INV_THREE_AXIS_COMPASS (0x0380) #define INV_ONE_AXIS_PRESSURE (0x0400) #define INV_ALL_SENSORS (0x7FFF) #ifdef INVENSENSE_COMPASS_CAL #define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \ | INV_THREE_AXIS_COMPASS \ | INV_THREE_AXIS_GYRO) #else #define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \ | INV_THREE_AXIS_COMPASS \ | INV_THREE_AXIS_GYRO) #endif // mask of virtual sensors that require gyro + accel + compass data #define VIRTUAL_SENSOR_9AXES_MASK ( \ (1 << Orientation) \ | (1 << RotationVector) \ | (1 << LinearAccel) \ | (1 << Gravity) \ ) // mask of virtual sensors that require gyro + accel data (but no compass data) #define VIRTUAL_SENSOR_GYRO_6AXES_MASK ( \ (1 << GameRotationVector) \ ) // mask of virtual sensors that require mag + accel data (but no gyro data) #define VIRTUAL_SENSOR_MAG_6AXES_MASK ( \ (1 << GeomagneticRotationVector) \ ) // mask of all virtual sensors #define VIRTUAL_SENSOR_ALL_MASK ( \ VIRTUAL_SENSOR_9AXES_MASK \ | VIRTUAL_SENSOR_GYRO_6AXES_MASK \ | VIRTUAL_SENSOR_MAG_6AXES_MASK \ ) // bit mask of current MPL active features (mMplFeatureActiveMask) #define INV_COMPASS_CAL 0x01 #define INV_COMPASS_FIT 0x02 // bit mask of current DMP active features (mFeatureActiveMask) #define INV_DMP_QUATERNION 0x001 //3 elements without real part, 32 bit each #define INV_DMP_DISPL_ORIENTATION 0x002 //screen orientation #define INV_DMP_SIGNIFICANT_MOTION 0x004 //significant motion #define INV_DMP_PEDOMETER 0x008 //interrupt-based pedometer #define INV_DMP_PEDOMETER_STEP 0x010 //timer-based pedometer #define INV_DMP_PED_STANDALONE 0x020 //timestamps only #define INV_DMP_6AXIS_QUATERNION 0x040 //3 elements without real part, 32 bit each #define INV_DMP_PED_QUATERNION 0x080 //3 elements without real part, 16 bit each #define INV_DMP_PED_INDICATOR 0x100 //tag along header with step indciator #define INV_DMP_BATCH_MODE 0x200 //batch mode // bit mask of whether DMP should be turned on #define DMP_FEATURE_MASK ( \ (INV_DMP_QUATERNION) \ | (INV_DMP_DISPL_ORIENTATION) \ | (INV_DMP_SIGNIFICANT_MOTION) \ | (INV_DMP_PEDOMETER) \ | (INV_DMP_PEDOMETER_STEP) \ | (INV_DMP_6AXIS_QUATERNION) \ | (INV_DMP_PED_QUATERNION) \ | (INV_DMP_BATCH_MODE) \ ) // bit mask of DMP features as sensors #define DMP_SENSOR_MASK ( \ (INV_DMP_DISPL_ORIENTATION) \ | (INV_DMP_SIGNIFICANT_MOTION) \ | (INV_DMP_PEDOMETER) \ | (INV_DMP_PEDOMETER_STEP) \ | (INV_DMP_6AXIS_QUATERNION) \ ) // data header format used by kernel driver. #define DATA_FORMAT_STEP 0x0001 #define DATA_FORMAT_MARKER 0x0010 #define DATA_FORMAT_EMPTY_MARKER 0x0020 #define DATA_FORMAT_PED_STANDALONE 0x0100 #define DATA_FORMAT_PED_QUAT 0x0200 #define DATA_FORMAT_6_AXIS 0x0400 #define DATA_FORMAT_QUAT 0x0800 #define DATA_FORMAT_COMPASS 0x1000 #define DATA_FORMAT_COMPASS_OF 0x1800 #define DATA_FORMAT_GYRO 0x2000 #define DATA_FORMAT_ACCEL 0x4000 #define DATA_FORMAT_PRESSURE 0x8000 #define DATA_FORMAT_MASK 0xffff #define BYTES_PER_SENSOR 8 #define BYTES_PER_SENSOR_PACKET 16 #define QUAT_ONLY_LAST_PACKET_OFFSET 16 #define BYTES_QUAT_DATA 24 #define MAX_READ_SIZE BYTES_QUAT_DATA #define MAX_SUSPEND_BATCH_PACKET_SIZE 1024 #define MAX_PACKET_SIZE 80 //8 * 4 + (2 * 24) /* Uncomment to enable Low Power Quaternion */ #define ENABLE_LP_QUAT_FEAT /* Enable Pressure sensor support */ #undef ENABLE_PRESSURE /* Screen Orientation is not currently supported */ int isDmpScreenAutoRotationEnabled() { #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION return 1; #else return 0; #endif } int (*m_pt2AccelCalLoadFunc)(long *bias) = NULL; /*****************************************************************************/ /** MPLSensor implementation which fits into the HAL example for crespo provided * by Google. * WARNING: there may only be one instance of MPLSensor, ever. */ class MPLSensor: public SensorBase { typedef int (MPLSensor::*hfunc_t)(sensors_event_t*); public: MPLSensor(CompassSensor *, int (*m_pt2AccelCalLoadFunc)(long*) = 0); virtual ~MPLSensor(); virtual int setDelay(int32_t handle, int64_t ns); virtual int enable(int32_t handle, int enabled); virtual int batch(int handle, int flags, int64_t period_ns, int64_t timeout); virtual int flush(int handle); int selectAndSetQuaternion(int batchMode, int mEnabled, long long featureMask); int checkBatchEnabled(); int setBatch(int en, int toggleEnable); int writeBatchTimeout(int en); int writeBatchTimeout(int en, int64_t timeoutInMs); int32_t getEnableMask() { return mEnabled; } void getHandle(int32_t handle, int &what, android::String8 &sname); virtual int readEvents(sensors_event_t *data, int count); virtual int getFd() const; virtual int getAccelFd() const; virtual int getCompassFd() const; virtual int getPollTime(); virtual int getStepCountPollTime(); virtual bool hasPendingEvents() const; virtual bool hasStepCountPendingEvents(); int populateSensorList(struct sensor_t *list, int len); void buildCompassEvent(); void buildMpuEvent(); int checkValidHeader(unsigned short data_format); int turnOffAccelFifo(); int turnOffGyroFifo(); int enableDmpOrientation(int); int dmpOrientHandler(int); int readDmpOrientEvents(sensors_event_t* data, int count); int getDmpOrientFd(); int openDmpOrientFd(); int closeDmpOrientFd(); int getDmpRate(int64_t *); int checkDMPOrientation(); int getDmpSignificantMotionFd(); int readDmpSignificantMotionEvents(sensors_event_t* data, int count); int enableDmpSignificantMotion(int); int significantMotionHandler(sensors_event_t* data); bool checkSmdSupport(){return (mDmpSignificantMotionEnabled);}; int enableDmpPedometer(int, int); int readDmpPedometerEvents(sensors_event_t* data, int count, int32_t id, int outputType); int getDmpPedometerFd(); bool checkPedometerSupport() {return (mDmpPedometerEnabled || mDmpStepCountEnabled);}; bool checkOrientationSupport() {return ((isDmpDisplayOrientationOn() && (mDmpOrientationEnabled || !isDmpScreenAutoRotationEnabled())));}; protected: CompassSensor *mCompassSensor; PressureSensor *mPressureSensor; int gyroHandler(sensors_event_t *data); int rawGyroHandler(sensors_event_t *data); int accelHandler(sensors_event_t *data); int compassHandler(sensors_event_t *data); int rawCompassHandler(sensors_event_t *data); int rvHandler(sensors_event_t *data); int grvHandler(sensors_event_t *data); int laHandler(sensors_event_t *data); int gravHandler(sensors_event_t *data); int orienHandler(sensors_event_t *data); int smHandler(sensors_event_t *data); int pHandler(sensors_event_t *data); int gmHandler(sensors_event_t *data); int psHandler(sensors_event_t *data); int sdHandler(sensors_event_t *data); int scHandler(sensors_event_t *data); int metaHandler(sensors_event_t *data, int flags); void calcOrientationSensor(float *Rx, float *Val); virtual int update_delay(); void inv_set_device_properties(); int inv_constructor_init(); int inv_constructor_default_enable(); int setAccelInitialState(); int masterEnable(int en); int enablePedStandalone(int en); int enablePedStandaloneData(int en); int enablePedQuaternion(int); int enablePedQuaternionData(int); int setPedQuaternionRate(int64_t wanted); int enable6AxisQuaternion(int); int enable6AxisQuaternionData(int); int set6AxisQuaternionRate(int64_t wanted); int enableLPQuaternion(int); int enableQuaternionData(int); int setQuaternionRate(int64_t wanted); int enableAccelPedometer(int); int enableAccelPedData(int); int onDmp(int); int enableGyro(int en); int enableLowPowerAccel(int en); int enableAccel(int en); int enableCompass(int en, int rawSensorOn); int enablePressure(int en); int enableBatch(int64_t timeout); void computeLocalSensorMask(int enabled_sensors); int computeBatchSensorMask(int enableSensor, int checkNewBatchSensor); int computeBatchDataOutput(); int enableSensors(unsigned long sensors, int en, uint32_t changed); int inv_read_temperature(long long *data); int inv_read_dmp_state(int fd); int inv_read_sensor_bias(int fd, long *data); void inv_get_sensors_orientation(void); int inv_init_sysfs_attributes(void); int resetCompass(void); void setCompassDelay(int64_t ns); void enable_iio_sysfs(void); int setDmpFeature(int en); int computeAndSetDmpState(void); int computeDmpState(bool* dmp_state); int SetDmpState(bool dmpState); int enablePedometer(int); int enablePedIndicator(int en); int checkPedStandaloneBatched(void); int checkPedStandaloneEnabled(void); int checkPedQuatEnabled(); int check6AxisQuatEnabled(); int checkLPQRateSupported(); int checkLPQuaternion(); int checkAccelPed(); void setInitial6QuatValue(); int writeSignificantMotionParams(bool toggleEnable, uint32_t delayThreshold1, uint32_t delayThreshold2, uint32_t motionThreshold); long mMasterSensorMask; long mLocalSensorMask; int mPollTime; int64_t mStepCountPollTime; bool mHaveGoodMpuCal; // flag indicating that the cal file can be written int mGyroAccuracy; // value indicating the quality of the gyro calibr. int mAccelAccuracy; // value indicating the quality of the accel calibr. int mCompassAccuracy; // value indicating the quality of the compass calibr. struct pollfd mPollFds[5]; pthread_mutex_t mMplMutex; pthread_mutex_t mHALMutex; char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH]; int iio_fd; int accel_fd; int mpufifo_fd; int gyro_temperature_fd; int accel_x_offset_fd; int accel_y_offset_fd; int accel_z_offset_fd; int accel_x_dmp_bias_fd; int accel_y_dmp_bias_fd; int accel_z_dmp_bias_fd; int gyro_x_offset_fd; int gyro_y_offset_fd; int gyro_z_offset_fd; int gyro_x_dmp_bias_fd; int gyro_y_dmp_bias_fd; int gyro_z_dmp_bias_fd; int dmp_orient_fd; int mDmpOrientationEnabled; int dmp_sign_motion_fd; int mDmpSignificantMotionEnabled; int dmp_pedometer_fd; int mDmpPedometerEnabled; int mDmpStepCountEnabled; uint32_t mEnabled; uint32_t mEnabledCached; uint32_t mBatchEnabled; android::Vector mFlushSensorEnabledVector; uint32_t mOldBatchEnabledMask; int64_t mBatchTimeoutInMs; sensors_event_t mPendingEvents[NumSensors]; sensors_event_t mPendingFlushEvents[NumSensors]; sensors_event_t mSmEvents; sensors_event_t mSdEvents; sensors_event_t mScEvents; int64_t mDelays[NumSensors]; int64_t mBatchDelays[NumSensors]; int64_t mBatchTimeouts[NumSensors]; hfunc_t mHandlers[NumSensors]; int64_t mEnabledTime[NumSensors]; int64_t mLastTimestamp[NumSensors]; short mCachedGyroData[3]; long mCachedAccelData[3]; long mCachedCompassData[3]; long mCachedQuaternionData[3]; long mCached6AxisQuaternionData[3]; long mCachedPedQuaternionData[3]; long mCachedPressureData; android::KeyedVector mIrqFds; InputEventCircularReader mAccelInputReader; InputEventCircularReader mGyroInputReader; int mCompassOverFlow; bool mFirstRead; short mTempScale; short mTempOffset; int64_t mTempCurrentTime; int mAccelScale; long mAccelSelfTestScale; long mGyroScale; long mGyroSelfTestScale; long mCompassScale; float mCompassBias[3]; bool mFactoryGyroBiasAvailable; long mFactoryGyroBias[3]; bool mGyroBiasAvailable; bool mGyroBiasApplied; float mGyroBias[3]; //in body frame long mGyroChipBias[3]; //in chip frame bool mFactoryAccelBiasAvailable; long mFactoryAccelBias[3]; bool mAccelBiasAvailable; bool mAccelBiasApplied; long mAccelBias[3]; //in chip frame uint32_t mPendingMask; unsigned long mSensorMask; char chip_ID[MAX_CHIP_ID_LEN]; char mSysfsPath[MAX_SYSFS_NAME_LEN]; signed char mGyroOrientation[9]; signed char mAccelOrientation[9]; int64_t mSensorTimestamp; int64_t mCompassTimestamp; int64_t mPressureTimestamp; int64_t mGyroBatchRate; int64_t mAccelBatchRate; int64_t mCompassBatchRate; int64_t mPressureBatchRate; int64_t mQuatBatchRate; int64_t mGyroRate; int64_t mAccelRate; int64_t mCompassRate; int64_t mPressureRate; int64_t mQuatRate; int64_t mResetRate; uint32_t mDataInterrupt; bool mFirstBatchCall; bool mEnableCalled; struct sysfs_attrbs { char *chip_enable; char *power_state; char *master_enable; char *dmp_firmware; char *firmware_loaded; char *dmp_on; char *dmp_int_on; char *dmp_event_int_on; char *tap_on; char *key; char *self_test; char *temperature; char *gyro_enable; char *gyro_fifo_rate; char *gyro_fsr; char *gyro_orient; char *gyro_fifo_enable; char *gyro_rate; char *accel_enable; char *accel_fifo_rate; char *accel_fsr; char *accel_bias; char *accel_orient; char *accel_fifo_enable; char *accel_rate; char *three_axis_q_on; //formerly quaternion_on char *three_axis_q_rate; char *six_axis_q_on; char *six_axis_q_rate; char *six_axis_q_value; char *ped_q_on; char *ped_q_rate; char *step_detector_on; char *step_indicator_on; char *in_timestamp_en; char *in_timestamp_index; char *in_timestamp_type; char *buffer_length; char *display_orientation_on; char *event_display_orientation; char *in_accel_x_offset; char *in_accel_y_offset; char *in_accel_z_offset; char *in_accel_self_test_scale; char *in_accel_x_dmp_bias; char *in_accel_y_dmp_bias; char *in_accel_z_dmp_bias; char *in_gyro_x_offset; char *in_gyro_y_offset; char *in_gyro_z_offset; char *in_gyro_self_test_scale; char *in_gyro_x_dmp_bias; char *in_gyro_y_dmp_bias; char *in_gyro_z_dmp_bias; char *event_smd; char *smd_enable; char *smd_delay_threshold; char *smd_delay_threshold2; char *smd_threshold; char *batchmode_timeout; char *batchmode_wake_fifo_full_on; char *flush_batch; char *pedometer_on; char *pedometer_int_on; char *event_pedometer; char *pedometer_steps; char *pedometer_step_thresh; char *pedometer_counter; char *motion_lpa_on; } mpu; char *sysfs_names_ptr; int mMplFeatureActiveMask; uint64_t mFeatureActiveMask; bool mDmpOn; int mPedUpdate; int mPressureUpdate; int64_t mQuatSensorTimestamp; int64_t mStepSensorTimestamp; uint64_t mLastStepCount; int mLeftOverBufferSize; char mLeftOverBuffer[1024]; bool mInitial6QuatValueAvailable; long mInitial6QuatValue[4]; int mFlushBatchSet; uint32_t mSkipReadEvents; uint32_t mSkipExecuteOnData; bool mDataMarkerDetected; bool mEmptyDataMarkerDetected; int mDmpState; private: /* added for dynamic get sensor list */ void fillAccel(const char* accel, struct sensor_t *list); void fillGyro(const char* gyro, struct sensor_t *list); void fillRV(struct sensor_t *list); void fillGMRV(struct sensor_t *list); void fillGRV(struct sensor_t *list); void fillOrientation(struct sensor_t *list); void fillGravity(struct sensor_t *list); void fillLinearAccel(struct sensor_t *list); void fillSignificantMotion(struct sensor_t *list); #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION void fillScreenOrientation(struct sensor_t *list); #endif void storeCalibration(); void loadDMP(); bool isMpuNonDmp(); int isLowPowerQuatEnabled(); int isDmpDisplayOrientationOn(); void getCompassBias(); void getFactoryGyroBias(); void setFactoryGyroBias(); void getGyroBias(); void setGyroZeroBias(); void setGyroBias(); void getFactoryAccelBias(); void setFactoryAccelBias(); void getAccelBias(); void setAccelBias(); int isCompassDisabled(); int setBatchDataRates(); int calcBatchDataRates(int64_t *gyro_rate, int64_t *accel_rate, int64_t *compass_rate, int64_t *pressure_rate, int64_t *quat_rate); int setBatchDataRates(int64_t gyroRate, int64_t accelRate, int64_t compassRate, int64_t pressureRate, int64_t quatRate); int resetDataRates(); int calctDataRates(int64_t *resetRate, int64_t *gyroRate, int64_t *accelRate, int64_t *compassRate, int64_t *pressureRate); int resetDataRates(int64_t resetRate, int64_t gyroRate, int64_t accelRate, int64_t compassRate, int64_t pressureRate); void initBias(); void resetMplStates(); void sys_dump(bool fileMode); int calcBatchTimeout(int en, int64_t *out); }; extern "C" { void setCallbackObject(MPLSensor*); MPLSensor *getCallbackObject(); } #endif // ANDROID_MPL_SENSOR_H