summaryrefslogtreecommitdiffstats
path: root/libsensors_iio/MPLSensor.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libsensors_iio/MPLSensor.cpp')
-rw-r--r--libsensors_iio/MPLSensor.cpp1136
1 files changed, 709 insertions, 427 deletions
diff --git a/libsensors_iio/MPLSensor.cpp b/libsensors_iio/MPLSensor.cpp
index e23ecc9..90051cd 100644
--- a/libsensors_iio/MPLSensor.cpp
+++ b/libsensors_iio/MPLSensor.cpp
@@ -34,6 +34,7 @@
#include <utils/String8.h>
#include <string.h>
#include <linux/input.h>
+#include <utils/Atomic.h>
#include "MPLSensor.h"
#include "MPLSupport.h"
@@ -46,7 +47,6 @@
#include "ml_sysfs_helper.h"
// #define TESTING
-// #define ENABLE_LP_QUAT_FEAT /* Uncomment to enable Low Power Quaternion */
#ifdef THIRD_PARTY_ACCEL
# warning "Third party accel"
@@ -80,11 +80,18 @@ static int hertz_request = 200;
#define HW_ACCEL_RATE_HZ (1000 / hertz_request)
#define HW_COMPASS_RATE_HZ (1000000000LL / hertz_request)
+#define RATE_200HZ 5000000LL
+#define RATE_15HZ 66667000LL
+#define RATE_5HZ 200000000LL
+
static struct sensor_t sSensorList[] =
{
{"MPL Gyroscope", "Invensense", 1,
SENSORS_GYROSCOPE_HANDLE,
SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
+ {"MPL Raw Gyro", "Invensense", 1,
+ SENSORS_RAW_GYROSCOPE_HANDLE,
+ SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
{"MPL Accelerometer", "Invensense", 1,
SENSORS_ACCELERATION_HANDLE,
SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, {}},
@@ -103,6 +110,12 @@ static struct sensor_t sSensorList[] =
{"MPL Gravity", "Invensense", 1,
SENSORS_GRAVITY_HANDLE,
SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, {}},
+
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+ {"MPL Screen Orientation", "Invensense ", 1,
+ SENSORS_SCREEN_ORIENTATION_HANDLE,
+ SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, {}},
+#endif
};
MPLSensor *MPLSensor::gMPLSensor = NULL;
@@ -134,17 +147,20 @@ static FILE *logfile = NULL;
* MPLSensor class implementation
******************************************************************************/
+// following extended initializer list would only be available with -std=c++11 or -std=gnu+11
MPLSensor::MPLSensor(CompassSensor *compass)
: SensorBase(NULL, NULL),
mMplSensorInitialized(false),
mNewData(0),
mMasterSensorMask(INV_ALL_SENSORS),
- mLocalSensorMask(ALL_MPL_SENSORS_NP),
+ mLocalSensorMask(0),
mPollTime(-1),
mHaveGoodMpuCal(0),
mGyroAccuracy(0),
mAccelAccuracy(0),
mSampleCount(0),
+ dmp_orient_fd(-1),
+ mDmpOrientationEnabled(0),
mEnabled(0),
mOldEnabledMask(0),
mAccelInputReader(4),
@@ -155,8 +171,6 @@ MPLSensor::MPLSensor(CompassSensor *compass)
mAccelScale(2),
mPendingMask(0),
mSensorMask(0),
- mGyroOrientation{0},
- mAccelOrientation{0},
mFeatureActiveMask(0) {
VFUNC_LOG;
@@ -175,6 +189,8 @@ MPLSensor::MPLSensor(CompassSensor *compass)
pthread_mutex_init(&mMplMutex, NULL);
pthread_mutex_init(&mHALMutex, NULL);
+ bzero(mGyroOrientation, sizeof(mGyroOrientation));
+ bzero(mAccelOrientation, sizeof(mAccelOrientation));
#ifdef INV_PLAYBACK_DBG
LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
@@ -204,11 +220,10 @@ MPLSensor::MPLSensor(CompassSensor *compass)
/* reset driver master enable */
masterEnable(0);
- /* Load DMP image if capable, ie. MPU6xxx/9xxx */
- // TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
- loadDMP();
-#endif
+ if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
+ /* Load DMP image if capable, ie. MPU6xxx/9xxx */
+ loadDMP();
+ }
/* open temperature fd for temp comp */
LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
@@ -269,6 +284,11 @@ MPLSensor::MPLSensor(CompassSensor *compass)
mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
+ mPendingEvents[RawGyro].version = sizeof(sensors_event_t);
+ mPendingEvents[RawGyro].sensor = ID_RG;
+ mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE;
+ mPendingEvents[RawGyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
+
mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
mPendingEvents[Accelerometer].sensor = ID_A;
mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
@@ -292,6 +312,7 @@ MPLSensor::MPLSensor(CompassSensor *compass)
mHandlers[LinearAccel] = &MPLSensor::laHandler;
mHandlers[Gravity] = &MPLSensor::gravHandler;
mHandlers[Gyro] = &MPLSensor::gyroHandler;
+ mHandlers[RawGyro] = &MPLSensor::rawGyroHandler;
mHandlers[Accelerometer] = &MPLSensor::accelHandler;
mHandlers[MagneticField] = &MPLSensor::compassHandler;
mHandlers[Orientation] = &MPLSensor::orienHandler;
@@ -306,7 +327,7 @@ MPLSensor::MPLSensor(CompassSensor *compass)
/* setup MPL */
inv_constructor_init();
- /* load calibration file from /data/cal.bin */
+ /* load calibration file from /data/inv_cal_data.bin */
rv = inv_load_calibration();
if(rv == INV_SUCCESS)
LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
@@ -320,8 +341,16 @@ MPLSensor::MPLSensor(CompassSensor *compass)
enableGyro(0);
enableAccel(0);
enableCompass(0);
+
+ if (isLowPowerQuatEnabled()) {
+ enableLPQuaternion(0);
+ }
+
onPower(0);
+ enableDmpOrientation(isDmpDisplayOrientationOn());
+ enableDmpOrientation(!isDmpScreenAutoRotationOn() && isDmpDisplayOrientationOn());
+
#ifdef INV_PLAYBACK_DBG
logfile = fopen("/data/playback.bin", "wb");
if (logfile)
@@ -331,56 +360,61 @@ MPLSensor::MPLSensor(CompassSensor *compass)
mMplSensorInitialized = true;
}
-
void MPLSensor::enable_iio_sysfs() {
VFUNC_LOG;
- int tempFd = 0;
char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
FILE *tempFp = NULL;
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:echo 1 > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo 1 > %s (%lld)",
mpu.in_timestamp_en, getTimestamp());
- tempFd = open(mpu.in_timestamp_en, O_RDWR);
- if(tempFd < 0) {
+ // Using fopen() here to benefit from fprintf()
+ tempFp = fopen(mpu.in_timestamp_en, "w");
+ if (tempFp == NULL) {
LOGE("HAL:could not open timestamp enable");
- } else if(enable_sysfs_sensor(tempFd, 1) < 0) {
- LOGE("HAL:could not enable timestamp");
+ } else {
+ if(fprintf(tempFp, "%d", 1) < 0) {
+ LOGE("HAL:could not enable timestamp");
+ }
+ fclose(tempFp);
}
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:cat %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE,
+ "HAL:sysfs:cat %s (%lld)",
mpu.trigger_name, getTimestamp());
tempFp = fopen(mpu.trigger_name, "r");
if (tempFp == NULL) {
LOGE("HAL:could not open trigger name");
- } else if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
- LOGE("HAL:could not read trigger name");
+ } else {
+ if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
+ LOGE("HAL:could not read trigger name");
+ }
+ fclose(tempFp);
}
- fclose(tempFp);
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:echo %s > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)",
iio_trigger_name, mpu.current_trigger, getTimestamp());
tempFp = fopen(mpu.current_trigger, "w");
if (tempFp == NULL) {
LOGE("HAL:could not open current trigger");
- } else if (fprintf(tempFp, "%s", iio_trigger_name) < 0) {
- LOGE("HAL:could not write current trigger");
+ } else {
+ if (fprintf(tempFp, "%s", iio_trigger_name) < 0) {
+ LOGE("HAL:could not write current trigger");
+ }
+ fclose(tempFp);
}
- fclose(tempFp);
- LOGV_IF(SYSFS_VERBOSE,
- "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp());
tempFp = fopen(mpu.buffer_length, "w");
if (tempFp == NULL) {
LOGE("HAL:could not open buffer length");
- } else if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) {
- LOGE("HAL:could not write buffer length");
+ } else {
+ if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) {
+ LOGE("HAL:could not write buffer length");
+ }
+ fclose(tempFp);
}
- fclose(tempFp);
inv_get_iio_device_node(iio_device_node);
iio_fd = open(iio_device_node, O_RDONLY);
@@ -428,7 +462,6 @@ int MPLSensor::inv_constructor_default_enable()
return result;
}
- // TODO: double-check for GED tablet
// result = inv_enable_motion_no_motion();
result = inv_enable_fast_nomot();
if (result) {
@@ -452,8 +485,13 @@ int MPLSensor::inv_constructor_default_enable()
if (result) {
LOG_RESULT_LOCATION(result);
return result;
+ } else {
+ mFeatureActiveMask |= INV_COMPASS_CAL;
}
+ // specify MPL's trust weight, used by compass algorithms
+ inv_vector_compass_cal_sensitivity(3);
+
result = inv_enable_compass_bias_w_gyro();
if (result) {
LOG_RESULT_LOCATION(result);
@@ -477,6 +515,9 @@ int MPLSensor::inv_constructor_default_enable()
if (result) {
LOG_RESULT_LOCATION(result);
return result;
+ } else {
+ // 9x sensor fusion enables Compass fit
+ mFeatureActiveMask |= INV_COMPASS_FIT;
}
result = inv_enable_no_gyro_fusion();
@@ -485,7 +526,6 @@ int MPLSensor::inv_constructor_default_enable()
return result;
}
- // TODO: double-check for GED tablet
result = inv_enable_quat_accuracy_monitor();
if (result) {
LOG_RESULT_LOCATION(result);
@@ -521,7 +561,7 @@ void MPLSensor::inv_set_device_properties()
/* compass setup */
signed char orientMtx[9];
mCompassSensor->getOrientationMatrix(orientMtx);
- orient =
+ orient =
inv_orientation_matrix_to_scalar(orientMtx);
long sensitivity;
sensitivity = mCompassSensor->getSensitivity();
@@ -539,7 +579,7 @@ void MPLSensor::loadDMP()
}
/* load DMP firmware */
- LOGV_IF(SYSFS_VERBOSE,
+ LOGV_IF(SYSFS_VERBOSE,
"HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
fd = open(mpu.firmware_loaded, O_RDONLY);
if(fd < 0) {
@@ -564,7 +604,7 @@ void MPLSensor::loadDMP()
}
}
- // onDMP(1); //Can't enable here. See note onDMP()
+ // onDMP(1); //Can't enable here. See note onDMP()
}
void MPLSensor::inv_get_sensors_orientation()
@@ -572,13 +612,13 @@ void MPLSensor::inv_get_sensors_orientation()
FILE *fptr;
// get gyro orientation
- LOGV_IF(SYSFS_VERBOSE,
+ LOGV_IF(SYSFS_VERBOSE,
"HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
fptr = fopen(mpu.gyro_orient, "r");
if (fptr != NULL) {
int om[9];
fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
- &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+ &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
&om[6], &om[7], &om[8]);
fclose(fptr);
@@ -601,13 +641,13 @@ void MPLSensor::inv_get_sensors_orientation()
}
// get accel orientation
- LOGV_IF(SYSFS_VERBOSE,
+ LOGV_IF(SYSFS_VERBOSE,
"HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
fptr = fopen(mpu.accel_orient, "r");
if (fptr != NULL) {
int om[9];
- fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
- &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
+ fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
+ &om[0], &om[1], &om[2], &om[3], &om[4], &om[5],
&om[6], &om[7], &om[8]);
fclose(fptr);
@@ -634,10 +674,11 @@ MPLSensor::~MPLSensor()
{
VFUNC_LOG;
+ mCompassSensor = NULL;
+
/* Close open fds */
if (iio_fd > 0)
close(iio_fd);
-
if( accel_fd > 0 )
close(accel_fd );
if (gyro_temperature_fd > 0)
@@ -645,18 +686,17 @@ MPLSensor::~MPLSensor()
if (sysfs_names_ptr)
free(sysfs_names_ptr);
+ if (isDmpDisplayOrientationOn()) {
+ closeDmpOrientFd();
+ }
+
/* Turn off Gyro master enable */
/* A workaround until driver handles it */
/* TODO: Turn off and close all sensors */
LOGV_IF(SYSFS_VERBOSE,
"HAL:sysfs:echo 0 > %s (%lld)", mpu.chip_enable, getTimestamp());
- int fd = open(mpu.chip_enable, O_RDWR);
- if(fd < 0) {
- LOGE("HAL:could not open gyro chip enable");
- } else {
- if(enable_sysfs_sensor(fd, 0) < 0) {
- LOGE("HAL:could not disable gyro master enable");
- }
+ if(write_sysfs_int(mpu.chip_enable, 0) < 0) {
+ LOGE("HAL:could not disable gyro master enable");
}
#ifdef INV_PLAYBACK_DBG
@@ -665,18 +705,13 @@ MPLSensor::~MPLSensor()
#endif
}
-#define GY_ENABLED ((1 << ID_GY) & enabled_sensors) // ID_GY = 0
-#define A_ENABLED ((1 << ID_A) & enabled_sensors) // ID_A = 1
-#ifdef INVENSENSE_COMPASS_CAL // ID_M = 2
-#define M_ENABLED ((1 << ID_M) & enabled_sensors)
-#else
-// TODO: ID_M = 2 even for 3rd-party solution
+#define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors)
+#define A_ENABLED ((1 << ID_A) & enabled_sensors)
#define M_ENABLED ((1 << ID_M) & enabled_sensors)
-#endif
-#define O_ENABLED ((1 << ID_O) & enabled_sensors) // ID_O = 3
-#define LA_ENABLED ((1 << ID_LA) & enabled_sensors) // ID_RV = 4
-#define GR_ENABLED ((1 << ID_GR) & enabled_sensors) // ID_LA = 5
-#define RV_ENABLED ((1 << ID_RV) & enabled_sensors) // ID_GR = 6
+#define O_ENABLED ((1 << ID_O) & enabled_sensors)
+#define LA_ENABLED ((1 << ID_LA) & enabled_sensors)
+#define GR_ENABLED ((1 << ID_GR) & enabled_sensors)
+#define RV_ENABLED ((1 << ID_RV) & enabled_sensors)
/* TODO: this step is optional, remove? */
int MPLSensor::setGyroInitialState()
@@ -685,7 +720,7 @@ int MPLSensor::setGyroInitialState()
int res = 0;
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp());
int fd = open(mpu.gyro_fifo_rate, O_RDWR);
res = errno;
@@ -733,38 +768,26 @@ int MPLSensor::onPower(int en)
{
VFUNC_LOG;
- int res = 0;
- char buf[sizeof(int)+1];
+ int res;
+
int count, curr_power_state;
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.power_state, getTimestamp());
- int tempFd = open(mpu.power_state, O_RDWR);
- res = errno;
- if(tempFd < 0){
- LOGE("HAL:Open of %s failed with '%s' (%d)",
- mpu.power_state, strerror(res), res);
- } else {
- // check and set new power state
- count = read_attribute_sensor(tempFd, buf, sizeof(buf));
- if(count < 1) {
- LOGE("HAL:Error reading power state");
- // will set power_state anyway
- curr_power_state= -1;
- } else {
- sscanf(buf, "%d", &curr_power_state);
- }
-
- if (en!=curr_power_state) {
- if((res=enable_sysfs_sensor(tempFd, en)) < 0) {
+ res = read_sysfs_int(mpu.power_state, &curr_power_state);
+ if (res < 0) {
+ LOGE("HAL:Error reading power state");
+ // will set power_state anyway
+ curr_power_state = -1;
+ }
+ if (en != curr_power_state) {
+ if((res = write_sysfs_int(mpu.power_state, en)) < 0) {
LOGE("HAL:Couldn't write power state");
- }
- } else {
- LOGV_IF(EXTRA_VERBOSE,
- "HAL:Power state already enable/disable curr=%d new=%d",
- curr_power_state, en);
- close(tempFd);
}
+ } else {
+ LOGV_IF(EXTRA_VERBOSE,
+ "HAL:Power state already enable/disable curr=%d new=%d",
+ curr_power_state, en);
}
return res;
}
@@ -773,7 +796,7 @@ int MPLSensor::onDMP(int en)
{
VFUNC_LOG;
- int res= -1;
+ int res = -1;
int status;
//Sequence to enable DMP
@@ -782,24 +805,32 @@ int MPLSensor::onDMP(int en)
//3. Either Gyro or Accel must be enabled/configured before next step
//4. Enable DMP
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+ mpu.firmware_loaded, getTimestamp());
if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){
LOGE("HAL:ERR can't get firmware_loaded status");
} else if (status == 1) {
//Write only if curr DMP state <> request
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)",
+ mpu.dmp_on, getTimestamp());
if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
LOGE("HAL:ERR can't read DMP state");
} else if (status != en) {
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ en, mpu.dmp_on, getTimestamp());
if (write_sysfs_int(mpu.dmp_on, en) < 0) {
- LOGE("HAL:ERR can't write dmp_on");
+ LOGE("HAL:ERR can't write dmp_on");
} else {
- res= 0; //Indicate write successful
+ res = 0; //Indicate write successful
}
//Enable DMP interrupt
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ en, mpu.dmp_int_on, getTimestamp());
if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
- LOGE("HAL:ERR can't en/dis DMP interrupt");
+ LOGE("HAL:ERR can't en/dis DMP interrupt");
}
} else {
- res= 0; //DMP already set as requested
+ res = 0; //DMP already set as requested
}
} else {
LOGE("HAL:ERR No DMP image");
@@ -824,7 +855,7 @@ int MPLSensor::enableLPQuaternion(int en)
mFeatureActiveMask &= ~INV_DMP_QUATERNION;
LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled");
} else {
- if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {
+ if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {
LOGE("HAL:ERR can't enable LP Quaternion");
} else {
mFeatureActiveMask |= INV_DMP_QUATERNION;
@@ -836,36 +867,52 @@ int MPLSensor::enableLPQuaternion(int en)
int MPLSensor::enableQuaternionData(int en)
{
- int res= 0;
+ int res = 0;
VFUNC_LOG;
- //Enable DMP quaternion
+ // Enable DMP quaternion
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ en, mpu.quaternion_on, getTimestamp());
if (write_sysfs_int(mpu.quaternion_on, en) < 0) {
LOGE("HAL:ERR can't write DMP quaternion_on");
- res= -1; //Indicate an err
- }
+ res = -1; //Indicate an err
+ }
if (!en) {
- LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
- inv_quaternion_sensor_was_turned_off();
+ LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems");
} else {
+
LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
- if (write_sysfs_int(mpu.in_quat_r_en, 1) < 0) {
- LOGE("HAL:ERR write in_quat_r_en");
- }
- if (write_sysfs_int(mpu.in_quat_x_en, 1) < 0) {
- LOGE("HAL:ERR write in_quat_x_en");
- }
- if (write_sysfs_int(mpu.in_quat_y_en, 1) < 0) {
- LOGE("HAL:ERR write in_quat_y_en");
- }
- if (write_sysfs_int(mpu.in_quat_z_en, 1) < 0) {
- LOGE("HAL:ERR write in_quat_z_en");
- }
}
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ en, mpu.in_quat_r_en, getTimestamp());
+ if (write_sysfs_int(mpu.in_quat_r_en, en) < 0) {
+ LOGE("HAL:ERR write in_quat_r_en");
+ }
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ en, mpu.in_quat_x_en, getTimestamp());
+ if (write_sysfs_int(mpu.in_quat_x_en, en) < 0) {
+ LOGE("HAL:ERR write in_quat_x_en");
+ }
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ en, mpu.in_quat_y_en, getTimestamp());
+ if (write_sysfs_int(mpu.in_quat_y_en, en) < 0) {
+ LOGE("HAL:ERR write in_quat_y_en");
+ }
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ en, mpu.in_quat_z_en, getTimestamp());
- return res;
+ if (write_sysfs_int(mpu.in_quat_z_en, en) < 0) {
+ LOGE("HAL:ERR write in_quat_z_en");
+ }
+
+ LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
+
+ if (!en) {
+ inv_quaternion_sensor_was_turned_off();
+ }
+ return res;
}
int MPLSensor::enableTap(int en)
@@ -893,75 +940,38 @@ int MPLSensor::masterEnable(int en)
{
VFUNC_LOG;
- int res = 0;
-
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.chip_enable, getTimestamp());
- int tempFd = open(mpu.chip_enable, O_RDWR);
- res = errno;
- if(tempFd < 0){
- LOGE("HAL:open of %s failed with '%s' (%d)",
- mpu.chip_enable, strerror(res), res);
- return res;
- }
- res = enable_sysfs_sensor(tempFd, en);
- return res;
+ return write_sysfs_int(mpu.chip_enable, en);
}
int MPLSensor::enableGyro(int en)
{
VFUNC_LOG;
- int res = 0;
+ /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
+ int res;
/* need to also turn on/off the master enable */
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.gyro_enable, getTimestamp());
- int tempFd = open(mpu.gyro_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- mpu.gyro_enable, strerror(res), res);
- }
+ res = write_sysfs_int(mpu.gyro_enable, en);
if (!en) {
LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
inv_gyro_was_turned_off();
} else {
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.gyro_x_fifo_enable, getTimestamp());
- tempFd = open(mpu.gyro_x_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- mpu.gyro_x_fifo_enable, strerror(res), res);
- }
+ write_sysfs_int(mpu.gyro_x_fifo_enable, en);
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.gyro_y_fifo_enable, getTimestamp());
- tempFd = open(mpu.gyro_y_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- mpu.gyro_y_fifo_enable, strerror(res), res);
- }
+ write_sysfs_int(mpu.gyro_y_fifo_enable, en);
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.gyro_z_fifo_enable, getTimestamp());
- tempFd = open(mpu.gyro_z_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- mpu.gyro_z_fifo_enable, strerror(res), res);
- }
+ res = write_sysfs_int(mpu.gyro_z_fifo_enable, en);
}
return res;
@@ -971,61 +981,31 @@ int MPLSensor::enableAccel(int en)
{
VFUNC_LOG;
+ /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
int res;
- /* need to also turn on/off the master enable */
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
- en, mpu.accel_enable, getTimestamp());
- int tempFd = open(mpu.accel_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- mpu.accel_enable, strerror(res), res);
- }
+ /* need to also turn on/off the master enable */
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ en, mpu.accel_enable, getTimestamp());
+ res = write_sysfs_int(mpu.accel_enable, en);
if (!en) {
LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
inv_accel_was_turned_off();
} else {
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.accel_x_fifo_enable, getTimestamp());
- tempFd = open(mpu.accel_x_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- mpu.accel_x_fifo_enable, strerror(res), res);
- }
+ write_sysfs_int(mpu.accel_x_fifo_enable, en);
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.accel_y_fifo_enable, getTimestamp());
- tempFd = open(mpu.accel_y_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- mpu.accel_y_fifo_enable, strerror(res), res);
- }
+ write_sysfs_int(mpu.accel_y_fifo_enable, en);
- LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
en, mpu.accel_z_fifo_enable, getTimestamp());
- tempFd = open(mpu.accel_z_fifo_enable, O_RDWR);
- res = errno;
- if (tempFd > 0) {
- res = enable_sysfs_sensor(tempFd, en);
- } else {
- LOGE("HAL:open of %s failed with '%s' (%d)",
- mpu.accel_z_fifo_enable, strerror(res), res);
- }
+ res = write_sysfs_int(mpu.accel_z_fifo_enable, en);
}
- if (!en && USE_THIRD_PARTY_ACCEL == 0) {
- }
-
if(USE_THIRD_PARTY_ACCEL == 1 && en) {
setAccelInitialState(); // BMA250
}
@@ -1040,7 +1020,7 @@ int MPLSensor::enableCompass(int en)
if (en == 0) {
LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
inv_compass_was_turned_off();
- }
+ }
return res;
}
@@ -1097,16 +1077,17 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
int off = 0;
// Sequence to enable or disable a sensor
- // 1. enable Power state
+ // 1. enable Power state
// 2. reset master enable (=0)
// 3. enable or disable a sensor
// 4. set master enable (=1)
- if (changed & ((1 << Gyro) | (1 << Accelerometer) |
- (mCompassSensor->isIntegrated() << MagneticField))) {
+ if (isLowPowerQuatEnabled() ||
+ changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
+ (mCompassSensor->isIntegrated() << MagneticField))) {
/* ensure power state is on */
onPower(1);
-
+
/* reset master enable */
res = masterEnable(0);
if(res < 0) {
@@ -1116,14 +1097,14 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);
- if (changed & (1 << Gyro)) {
- if(sensors & INV_THREE_AXIS_GYRO) {
+ if (changed & ((1 << Gyro) | (1 << RawGyro))) {
+ if (sensors & INV_THREE_AXIS_GYRO) {
LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable gyro");
res = enableGyro(on);
if(res < 0) {
return res;
}
- } else if((sensors & INV_THREE_AXIS_GYRO) == 0) {
+ } else if ((sensors & INV_THREE_AXIS_GYRO) == 0) {
LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable gyro");
res = enableGyro(off);
if(res < 0) {
@@ -1133,13 +1114,13 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
}
if (changed & (1 << Accelerometer)) {
- if(sensors & INV_THREE_AXIS_ACCEL) {
+ if (sensors & INV_THREE_AXIS_ACCEL) {
LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable accel");
res = enableAccel(on);
if(res < 0) {
return res;
}
- } else if((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+ } else if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable accel");
res = enableAccel(off);
if(res < 0) {
@@ -1165,47 +1146,101 @@ int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
}
}
-// TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
- // Enable LP Quat
- if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
- (1 << LinearAccel) | (1 << Gravity)))) {
- if (!checkLPQuaternion()) {
- enableLPQuaternion(1);
- } else {
- LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
+ if ( isLowPowerQuatEnabled() ) {
+ // Enable LP Quat
+ if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
+ (1 << LinearAccel) | (1 << Gravity)))) {
+ if (!(changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
+ (mCompassSensor->isIntegrated() << MagneticField)))) {
+ /* ensure power state is on */
+ onPower(1);
+ /* reset master enable */
+ res = masterEnable(0);
+ if(res < 0) {
+ return res;
+ }
+ }
+ if (!checkLPQuaternion()) {
+ enableLPQuaternion(1);
+ } else {
+ LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
+ }
+ } else if (checkLPQuaternion()) {
+ enableLPQuaternion(0);
}
- } else if (checkLPQuaternion()) {
- enableLPQuaternion(0);
}
-#endif
- /*
- if sensor & THREE_AXIS_GYRO
- enable = 1
- if sensor & THREE_AXIS_ACCEL
- enable = 1
- if compass_on_secondary
- if sensor & THREE_AXIS_COMPASS
- enable = 1
- else
- enable = 0
- */
- if (changed & ((1 << Gyro) | (1 << Accelerometer) |
+ if (changed & ((1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) |
(mCompassSensor->isIntegrated() << MagneticField))) {
- if (sensors &
- (INV_THREE_AXIS_GYRO
- | INV_THREE_AXIS_ACCEL
+ if (sensors &
+ (INV_THREE_AXIS_GYRO
+ | INV_THREE_AXIS_ACCEL
| (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
+ if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
+ // disable DMP event interrupt only (w/ data interrupt)
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ 0, mpu.dmp_event_int_on, getTimestamp());
+ if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) {
+ res = -1;
+ LOGE("HAL:ERR can't disable DMP event interrupt");
+ return res;
+ }
+ }
+
+ if (isDmpDisplayOrientationOn()) {
+ // enable DMP
+ onDMP(1);
+
+ res = enableAccel(on);
+ if(res < 0) {
+ return res;
+ }
+
+ if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+ res = turnOffAccelFifo();
+ }
+ if(res < 0) {
+ return res;
+ }
+ }
+
res = masterEnable(1);
if(res < 0) {
return res;
}
} else { // all sensors idle -> reduce power
- res = onPower(0);
- if(res < 0) {
- return res;
+ if (isDmpDisplayOrientationOn()) {
+ // enable DMP
+ onDMP(1);
+ // enable DMP event interrupt only (no data interrupt)
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ 1, mpu.dmp_event_int_on, getTimestamp());
+ if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) {
+ res = -1;
+ LOGE("HAL:ERR can't enable DMP event interrupt");
+ }
+ res = enableAccel(on);
+ if(res < 0) {
+ return res;
+ }
+ if ((sensors & INV_THREE_AXIS_ACCEL) == 0) {
+ res = turnOffAccelFifo();
+ }
+ if(res < 0) {
+ return res;
+ }
+ res = masterEnable(1);
+ if(res < 0) {
+ return res;
+ }
+ }
+ else {
+ res = onPower(0);
+ if(res < 0) {
+ return res;
+ }
}
+
storeCalibration();
}
}
@@ -1244,6 +1279,16 @@ int MPLSensor::gyroHandler(sensors_event_t* s)
return update;
}
+int MPLSensor::rawGyroHandler(sensors_event_t* s)
+{
+ VHANDLER_LOG;
+ int update;
+ update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &s->gyro.status, &s->timestamp);
+ LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d",
+ s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
+ return update;
+}
+
int MPLSensor::accelHandler(sensors_event_t* s)
{
VHANDLER_LOG;
@@ -1251,7 +1296,7 @@ int MPLSensor::accelHandler(sensors_event_t* s)
update = inv_get_sensor_type_accelerometer(
s->acceleration.v, &s->acceleration.status, &s->timestamp);
LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
- s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
+ s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2],
s->timestamp, update);
mAccelAccuracy = s->acceleration.status;
return update;
@@ -1317,9 +1362,17 @@ int MPLSensor::enable(int32_t handle, int en)
VFUNC_LOG;
android::String8 sname;
- int what = -1;
+ int what = -1, err = 0;
switch (handle) {
+ case ID_SO:
+ sname = "Screen Orientation";
+ LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
+ (mDmpOrientationEnabled? "en": "dis"),
+ (en? "en" : "dis"));
+ enableDmpOrientation(en && isDmpDisplayOrientationOn());
+ mDmpOrientationEnabled = !!en;
+ return 0;
case ID_A:
what = Accelerometer;
sname = "Accelerometer";
@@ -1336,6 +1389,10 @@ int MPLSensor::enable(int32_t handle, int en)
what = Gyro;
sname = "Gyro";
break;
+ case ID_RG:
+ what = RawGyro;
+ sname = "RawGyro";
+ break;
case ID_GR:
what = Gravity;
sname = "Gravity";
@@ -1358,7 +1415,6 @@ int MPLSensor::enable(int32_t handle, int en)
return -EINVAL;
int newState = en ? 1 : 0;
- int err = 0;
unsigned long sen_mask;
LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
@@ -1367,7 +1423,6 @@ int MPLSensor::enable(int32_t handle, int en)
LOGV_IF(PROCESS_VERBOSE,
"HAL:%s sensor state change what=%d", sname.string(), what);
- // TODO: disabled for GED tablet
// pthread_mutex_lock(&mMplMutex);
// pthread_mutex_lock(&mHALMutex);
@@ -1389,6 +1444,7 @@ int MPLSensor::enable(int32_t handle, int en)
switch (what) {
case Gyro:
+ case RawGyro:
case Accelerometer:
case MagneticField:
if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
@@ -1422,7 +1478,7 @@ int MPLSensor::enable(int32_t handle, int en)
// pthread_mutex_unlock(&mHALMutex);
#ifdef INV_PLAYBACK_DBG
- /* apparently the logging needs to be go through this sequence
+ /* apparently the logging needs to be go through this sequence
to properly flush the log file */
inv_turn_off_data_logging();
fclose(logfile);
@@ -1442,6 +1498,8 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
int what = -1;
switch (handle) {
+ case ID_SO:
+ return 0;
case ID_A:
what = Accelerometer;
sname = "Accelerometer";
@@ -1458,6 +1516,10 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
what = Gyro;
sname = "Gyro";
break;
+ case ID_RG:
+ what = RawGyro;
+ sname = "RawGyro";
+ break;
case ID_GR:
what = Gravity;
sname = "Gravity";
@@ -1476,7 +1538,6 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
break;
}
-// TODO: disabled for GED tablet
#if 0
// skip the 1st call for enalbing sensors called by ICS/JB sensor service
static int counter_delay = 0;
@@ -1500,7 +1561,6 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);
- // TODO: for GED tablet
// limit all rates to reasonable ones */
/*
if (ns < 10000000LL) {
@@ -1516,6 +1576,7 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
switch (what) {
case Gyro:
+ case RawGyro:
case Accelerometer:
for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated();
i++) {
@@ -1529,6 +1590,7 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
case MagneticField:
if (mCompassSensor->isIntegrated() &&
(((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
+ ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) ||
((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) {
LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel");
return 0;
@@ -1539,6 +1601,11 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
case RotationVector:
case LinearAccel:
case Gravity:
+ if (isLowPowerQuatEnabled()) {
+ LOGV_IF(PROCESS_VERBOSE, "HAL:need to update delay due to LPQ");
+ break;
+ }
+
for (int i = 0; i < numSensors; i++) {
if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
@@ -1548,7 +1615,6 @@ int MPLSensor::setDelay(int32_t handle, int64_t ns)
break;
}
- // TODO: disabled for GED tablet
// pthread_mutex_lock(&mHALMutex);
int res = update_delay();
// pthread_mutex_unlock(&mHALMutex);
@@ -1562,33 +1628,31 @@ int MPLSensor::update_delay() {
int64_t got;
if (mEnabled) {
- uint64_t wanted = -1LLU;
- uint64_t wanted_ec = -1LLU;
+ int64_t wanted = 1000000000;
+ int64_t wanted_3rd_party_sensor = 1000000000;
- // Sequence to change sensor's FIFO rate
- // 1. enable Power state
+ // Sequence to change sensor's FIFO rate
+ // 1. enable Power state
// 2. reset master enable
// 3. Update delay
// 4. set master enable
- // TODO: unnecessary for IIO
// ensure power on
- // onPower(1);
+ onPower(1);
- // TODO: unnecessary for IIO
// reset master enable
- // masterEnable(0);
+ masterEnable(0);
/* search the minimum delay requested across all enabled sensors */
for (int i = 0; i < numSensors; i++) {
if (mEnabled & (1 << i)) {
- uint64_t ns = mDelays[i];
+ int64_t ns = mDelays[i];
wanted = wanted < ns ? wanted : ns;
}
}
- // same delay for ext compass
- wanted_ec = wanted;
+ // same delay for 3rd party Accel or Compass
+ wanted_3rd_party_sensor = wanted;
/* mpl rate in us in future maybe different for
gyro vs compass vs accel */
@@ -1607,25 +1671,6 @@ int MPLSensor::update_delay() {
inv_set_accel_sample_rate(mplAccelRate);
inv_set_compass_sample_rate(mplCompassRate);
-// TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
- // Set LP Quaternion sample rate if enabled
- if (checkLPQuaternion()) {
- // TODO: need to verify this part for LPQ
- if (wanted < 5000000LL) {
- enableLPQuaternion(0);
- } else {
- inv_set_quat_sample_rate(rateInus);
- // set DMP output rate to FIFO
- write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / wanted);
-
- //DMP running rate must be @ 200Hz
- wanted= 5000000LL;
- LOGV("HAL:DMP rate= %.2f Hz Fifo Rate= %d us", 1000000000.f / wanted, rateInus);
- }
- }
-#endif
-
/* TODO: Test 200Hz */
// inv_set_gyro_sample_rate(5000);
LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate);
@@ -1635,11 +1680,28 @@ int MPLSensor::update_delay() {
int enabled_sensors = mEnabled;
int tempFd = -1;
if(LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
- uint64_t tempRate = wanted;
+ if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) {
+ bool setDMPrate= 0;
+ // Set LP Quaternion sample rate if enabled
+ if (checkLPQuaternion()) {
+ if (wanted < RATE_200HZ) {
+ enableLPQuaternion(0);
+ } else {
+ inv_set_quat_sample_rate(rateInus);
+ setDMPrate= 1;
+ }
+ }
+
+ if (checkDMPOrientation() || setDMPrate==1) {
+ getDmpRate(&wanted);
+ }
+ }
+
+ int64_t tempRate = wanted;
LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
//nsToHz
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
- 1000000000.f / tempRate, mpu.gyro_fifo_rate,
+ 1000000000.f / tempRate, mpu.gyro_fifo_rate,
getTimestamp());
tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
res = write_attribute_sensor(tempFd, 1000000000.f / tempRate);
@@ -1650,15 +1712,16 @@ int MPLSensor::update_delay() {
//nsToHz (BMA250)
if(USE_THIRD_PARTY_ACCEL == 1) {
LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
- wanted / 1000000L, mpu.accel_fifo_rate,
+ wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate,
getTimestamp());
tempFd = open(mpu.accel_fifo_rate, O_RDWR);
- res = write_attribute_sensor(tempFd, wanted / 1000000L);
+ res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L);
LOGE_IF(res < 0, "HAL:ACCEL update delay error");
}
if (!mCompassSensor->isIntegrated()) {
- mCompassSensor->setDelay(ID_M, wanted_ec);
+ LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz", 1000000000.f / wanted_3rd_party_sensor);
+ mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor);
got = mCompassSensor->getDelay(ID_M);
inv_set_compass_sample_rate(got / 1000);
}
@@ -1679,7 +1742,14 @@ int MPLSensor::update_delay() {
} else {
if (GY_ENABLED) {
- wanted = mDelays[Gyro];
+ wanted = (mDelays[Gyro] <= mDelays[RawGyro]?
+ (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]):
+ (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro]));
+
+ if (isDmpDisplayOrientationOn()) {
+ getDmpRate(&wanted);
+ }
+
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp());
tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
@@ -1690,9 +1760,18 @@ int MPLSensor::update_delay() {
if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */
if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
wanted = mDelays[Gyro];
+ }
+ else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) {
+ wanted = mDelays[RawGyro];
+
} else {
wanted = mDelays[Accelerometer];
}
+
+ if (isDmpDisplayOrientationOn()) {
+ getDmpRate(&wanted);
+ }
+
/* TODO: use function pointers to calculate delay value specific to vendor */
LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp());
@@ -1711,12 +1790,20 @@ int MPLSensor::update_delay() {
} else {
if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) {
wanted = mDelays[Gyro];
- } else if (GY_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
+ }
+ else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) {
+ wanted = mDelays[RawGyro];
+ } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
wanted = mDelays[Accelerometer];
} else {
wanted = mDelays[MagneticField];
}
+
+ if (isDmpDisplayOrientationOn()) {
+ getDmpRate(&wanted);
+ }
}
+
mCompassSensor->setDelay(ID_M, wanted);
got = mCompassSensor->getDelay(ID_M);
inv_set_compass_sample_rate(got / 1000);
@@ -1724,37 +1811,20 @@ int MPLSensor::update_delay() {
}
- /*
- if sensor & THREE_AXIS_GYRO
- enable = 1
- if sensor & THREE_AXIS_ACCEL
- enable = 1
- if compass_on_secondary
- if sensor & THREE_AXIS_COMPASS
- enable = 1
- else
- enable = 0
- */
unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
- if (sensors &
- (INV_THREE_AXIS_GYRO
- | INV_THREE_AXIS_ACCEL
+ if (sensors &
+ (INV_THREE_AXIS_GYRO
+ | INV_THREE_AXIS_ACCEL
| (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
-// TODO: unnecessary for IIO
-#if 0
res = masterEnable(1);
if(res < 0) {
return res;
}
-#endif
} else { // all sensors idle -> reduce power
-// TODO: unnecessary for IIO
-#if 0
res = onPower(0);
if(res < 0) {
return res;
}
-#endif
}
}
@@ -1762,6 +1832,7 @@ int MPLSensor::update_delay() {
}
/* use for third party accel */
+/* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */
int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
{
VHANDLER_LOG;
@@ -1804,17 +1875,17 @@ int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
mAccelInputReader.next();
}
- LOGV_IF(ENG_VERBOSE, "HAL:readAccelEvents - events read=%d", numEventReceived);
-
return numEventReceived;
}
-/**
+/**
* Should be called after reading at least one of gyro
- * compass or accel data. You should only read 1 sample of
- * data and call this.
+ * compass or accel data. (Also okay for handling all of them).
* @returns 0, if successful, error number if not.
*/
+/* TODO: This should probably be called "int readEvents(...)"
+ * and readEvents() called "void cacheData(void)".
+ */
int MPLSensor::executeOnData(sensors_event_t* data, int count)
{
VFUNC_LOG;
@@ -1858,36 +1929,40 @@ int MPLSensor::executeOnData(sensors_event_t* data, int count)
return numEventReceived;
}
+// collect data for MPL (but NOT sensor service currently), from driver layer
+/* TODO: FIX! data and count are not used, results is hardcoded to 0 */
+/* TODO: This should probably be called "void cacheEvents(void)"
+ * And executeOnData() should be int readEvents(data,count)
+ */
int MPLSensor::readEvents(sensors_event_t *data, int count) {
int lp_quaternion_on, nbyte;
int i, nb, mask = 0, numEventReceived = 0,
- sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) +
- ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1: 0) +
- (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1: 0);
+ sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) +
+ ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) +
+ (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1 : 0);
char *rdata = mIIOBuffer;
nbyte= (8 * sensors + 8) * 1;
-// TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
- lp_quaternion_on = checkLPQuaternion();
- if (lp_quaternion_on == 1) {
- nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data
+ if (isLowPowerQuatEnabled()) {
+ lp_quaternion_on = checkLPQuaternion();
+ if (lp_quaternion_on == 1) {
+ nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data
+ }
}
-#endif
- // TODO: disabled for GED tablet
// pthread_mutex_lock(&mMplMutex);
// pthread_mutex_lock(&mHALMutex);
- size_t rsize = read(iio_fd, rdata, nbyte);
+ ssize_t rsize = read(iio_fd, rdata, nbyte);
if (sensors == 0) {
// read(iio_fd, rdata, nbyte);
- read(iio_fd, rdata, (16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH);
+ rsize = read(iio_fd, rdata, sizeof(mIIOBuffer));
}
-/*
+
+#ifdef TESTING
LOGI("get one sample of IIO data with size: %d", rsize);
LOGI("sensors: %d", sensors);
@@ -1907,27 +1982,21 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
*((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
-*/
+#endif
- // TODO: need to verify this for LPQ
if (rsize < (nbyte - 8)) {
- LOGE("HAL:ERR Full data packet was not read");
- // return -1;
+ LOGE("HAL:ERR Full data packet was not read. rsize=%ld nbyte=%d sensors=%d errno=%d(%s)",
+ rsize, nbyte, sensors, errno, strerror(errno));
+ return -1;
}
-// TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
- if (lp_quaternion_on == 1) {
+ if (isLowPowerQuatEnabled() && lp_quaternion_on == 1) {
+
for (i=0; i< 4; i++) {
mCachedQuaternionData[i]= *(long*)rdata;
rdata += sizeof(long);
}
}
-/*
- LOGV("HAL:rdata= %x sensors= %d lp_q_on= %d nbyte= %d rsize= %d",
- rdata, sensors, lp_quaternion_on, nbyte, rsize); //tbd
-*/
-#endif
for (i = 0; i < 3; i++) {
if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
@@ -1942,11 +2011,11 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
}
}
- mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) +
- ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 2: 0));
+ mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) +
+ ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0));
if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
(mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
- mask |= 4;
+ mask |= 1 << MagneticField;
}
mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
@@ -1954,58 +2023,61 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
mCompassTimestamp = mSensorTimestamp;
}
- // send down temperature every 0.5 seconds
- if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
- mTempCurrentTime = mSensorTimestamp;
- long long temperature[2];
- if(inv_read_temperature(temperature) == 0) {
- LOGV_IF(INPUT_DATA,
- "HAL:inv_read_temperature = %lld, timestamp= %lld",
- temperature[0], temperature[1]);
- inv_build_temp(temperature[0], temperature[1]);
- }
+ if (mask & (1 << Gyro)) {
+ // send down temperature every 0.5 seconds
+ // with timestamp measured in "driver" layer
+ if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
+ mTempCurrentTime = mSensorTimestamp;
+ long long temperature[2];
+ if(inv_read_temperature(temperature) == 0) {
+ LOGV_IF(INPUT_DATA,
+ "HAL:inv_read_temperature = %lld, timestamp= %lld",
+ temperature[0], temperature[1]);
+ inv_build_temp(temperature[0], temperature[1]);
+ }
#ifdef TESTING
- long bias[3], temp, temp_slope[3];
- inv_get_gyro_bias(bias, &temp);
- inv_get_gyro_ts(temp_slope);
-
- LOGI("T: %.3f "
- "GB: %+13f %+13f %+13f "
- "TS: %+13f %+13f %+13f "
- "\n",
- (float)temperature[0] / 65536.f,
- (float)bias[0] / 65536.f / 16.384f,
- (float)bias[1] / 65536.f / 16.384f,
- (float)bias[2] / 65536.f / 16.384f,
- temp_slope[0] / 65536.f,
- temp_slope[1] / 65536.f,
- temp_slope[2] / 65536.f);
+ long bias[3], temp, temp_slope[3];
+ inv_get_gyro_bias(bias, &temp);
+ inv_get_gyro_ts(temp_slope);
+
+ LOGI("T: %.3f "
+ "GB: %+13f %+13f %+13f "
+ "TS: %+13f %+13f %+13f "
+ "\n",
+ (float)temperature[0] / 65536.f,
+ (float)bias[0] / 65536.f / 16.384f,
+ (float)bias[1] / 65536.f / 16.384f,
+ (float)bias[2] / 65536.f / 16.384f,
+ temp_slope[0] / 65536.f,
+ temp_slope[1] / 65536.f,
+ temp_slope[2] / 65536.f);
#endif
- }
+ }
- if (mask & 1) {
mPendingMask |= 1 << Gyro;
+ mPendingMask |= 1 << RawGyro;
+
if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
inv_build_gyro(mCachedGyroData, mSensorTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:inv_build_gyro: %+8d %+8d %+8d - %lld",
- mCachedGyroData[0], mCachedGyroData[1],
+ mCachedGyroData[0], mCachedGyroData[1],
mCachedGyroData[2], mSensorTimestamp);
}
}
- if (mask & 2) {
+ if (mask & (1 << Accelerometer)) {
mPendingMask |= 1 << Accelerometer;
if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
inv_build_accel(mCachedAccelData, 0, mSensorTimestamp);
LOGV_IF(INPUT_DATA,
"HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld",
- mCachedAccelData[0], mCachedAccelData[1],
+ mCachedAccelData[0], mCachedAccelData[1],
mCachedAccelData[2], mSensorTimestamp);
}
}
- if ((mask & 4) && mCompassSensor->isIntegrated()) {
+ if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) {
int status = 0;
if (mCompassSensor->providesCalibration()) {
status = mCompassSensor->getAccuracy();
@@ -2014,21 +2086,19 @@ int MPLSensor::readEvents(sensors_event_t *data, int count) {
if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
inv_build_compass(mCachedCompassData, status,
mCompassTimestamp);
- LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
- mCachedCompassData[0], mCachedCompassData[1],
+ LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+ mCachedCompassData[0], mCachedCompassData[1],
mCachedCompassData[2], mCompassTimestamp);
}
}
-// TODO: disabled for GED tablet
-#ifdef ENABLE_LP_QUAT_FEAT
- if (lp_quaternion_on == 1) {
+ if (isLowPowerQuatEnabled() && lp_quaternion_on == 1) {
+
inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp);
- LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld - %d",
- mCachedQuaternionData[0], mCachedQuaternionData[1],
+ LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld",
+ mCachedQuaternionData[0], mCachedQuaternionData[1],
mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
}
-#endif
// pthread_mutex_unlock(&mMplMutex);
// pthread_mutex_unlock(&mHALMutex);
@@ -2048,12 +2118,16 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
int done = 0;
int nb;
- // TODO: disabled for GED tablet
- // TODO: for AMI306
// pthread_mutex_lock(&mMplMutex);
// pthread_mutex_lock(&mHALMutex);
done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
+#ifdef COMPASS_YAS530
+ if (mCompassSensor->checkCoilsReset()==1) {
+ //Reset relevant compass settings
+ resetCompass();
+ }
+#endif
if (done > 0) {
int status = 0;
if (mCompassSensor->providesCalibration()) {
@@ -2063,8 +2137,8 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
inv_build_compass(mCachedCompassData, status,
mCompassTimestamp);
- LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
- mCachedCompassData[0], mCachedCompassData[1],
+ LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld",
+ mCachedCompassData[0], mCachedCompassData[1],
mCachedCompassData[2], mCompassTimestamp);
}
}
@@ -2075,6 +2149,27 @@ int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
return numEventReceived;
}
+#ifdef COMPASS_YAS530
+int MPLSensor::resetCompass()
+{
+ VFUNC_LOG;
+
+ //Reset compass cal if enabled
+ if (mFeatureActiveMask & INV_COMPASS_CAL) {
+ LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal");
+ inv_init_vector_compass_cal();
+ }
+
+ //Reset compass fit if enabled
+ if (mFeatureActiveMask & INV_COMPASS_FIT) {
+ LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit");
+ inv_init_compass_fit();
+ }
+
+ return 0;
+}
+#endif
+
int MPLSensor::getFd() const
{
VFUNC_LOG;
@@ -2097,6 +2192,191 @@ int MPLSensor::getCompassFd() const
return fd;
}
+int MPLSensor::turnOffAccelFifo() {
+ int i, res, tempFd;
+ char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable,
+ mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable};
+
+ for (i = 0; i < 3; i++) {
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ 0, accel_fifo_enable[i], getTimestamp());
+ res = write_sysfs_int(accel_fifo_enable[i], 0);
+ if (res < 0) {
+ return res;
+ }
+ }
+ return 0;
+}
+
+int MPLSensor::enableDmpOrientation(int en)
+{
+ VFUNC_LOG;
+ /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */
+ int res = 0;
+
+ // on power if not already On
+ onPower(1);
+ // reset master enable
+ masterEnable(0);
+
+ if (en == 1) {
+ //Enable DMP orientation
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ en, mpu.display_orientation_on, getTimestamp());
+ if (write_sysfs_int(mpu.display_orientation_on, en) < 0) {
+ LOGE("HAL:ERR can't enable Android orientation");
+ res = -1; //Indicate an err
+ }
+
+ //Open DMP Orient Fd
+ openDmpOrientFd();
+
+ //Enable DMP
+ onDMP(1);
+
+ //Default DMP output rate to FIFO
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ 5, mpu.dmp_output_rate, getTimestamp());
+ if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) {
+ LOGE("HAL:ERR can't default DMP output rate");
+ }
+
+ //Set DMP rate to 200Hz
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ 200, mpu.accel_fifo_rate, getTimestamp());
+ if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) {
+ res = -1;
+ LOGE("HAL:ERR can't set DMP rate to 200Hz");
+ }
+
+ // enable accel engine
+ enableAccel(1);
+
+ // disable accel FIFO
+ res = turnOffAccelFifo();
+
+ mFeatureActiveMask |=INV_DMP_DISPL_ORIENTATION;
+ } else {
+ // disable DMP
+ onDMP(0);
+ // disable accel engine
+ enableAccel(0);
+ }
+
+ res = masterEnable(1);
+ return res;
+}
+
+int MPLSensor::openDmpOrientFd()
+{
+ VFUNC_LOG;
+
+ if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) {
+ LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened");
+ return -1;
+ }
+
+ dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK);
+ if (dmp_orient_fd < 0) {
+ LOGE("HAL:ERR couldn't open dmpOrient node");
+ return -1;
+ } else {
+ LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_orient_fd opened : %d", dmp_orient_fd);
+ return 0;
+ }
+}
+
+int MPLSensor::closeDmpOrientFd()
+{
+ VFUNC_LOG;
+ if (dmp_orient_fd >= 0)
+ close(dmp_orient_fd);
+ return 0;
+}
+
+int MPLSensor::dmpOrientHandler(int orient)
+{
+ VFUNC_LOG;
+
+ LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient);
+ return 0;
+}
+
+int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) {
+ VFUNC_LOG;
+
+ char dummy[4];
+ int screen_orientation = 0;
+ FILE *fp;
+
+ fp = fopen(mpu.event_display_orientation, "r");
+ if (fp == NULL) {
+ LOGE("HAL:cannot open event_display_orientation");
+ return 0;
+ }
+ fscanf(fp, "%d\n", &screen_orientation);
+ fclose(fp);
+
+ int numEventReceived = 0;
+
+ if (mDmpOrientationEnabled && count > 0) {
+ sensors_event_t temp;
+
+ bzero(&temp, sizeof(temp)); /* Let's hope that SENSOR_TYPE_NONE is 0 */
+ temp.version = sizeof(sensors_event_t);
+ temp.sensor = ID_SO;
+#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION
+ temp.type = SENSOR_TYPE_SCREEN_ORIENTATION;
+ temp.screen_orientation = screen_orientation;
+#endif
+ struct timespec ts;
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
+
+ *data++ = temp;
+ count--;
+ numEventReceived++;
+ }
+
+ // read dummy data per driver's request
+ dmpOrientHandler(screen_orientation);
+ read(dmp_orient_fd, dummy, 4);
+
+ return numEventReceived;
+}
+
+int MPLSensor::getDmpOrientFd()
+{
+ VFUNC_LOG;
+
+ LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd);
+ return dmp_orient_fd;
+
+}
+
+int MPLSensor::checkDMPOrientation()
+{
+ VFUNC_LOG;
+ return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0);
+}
+
+int MPLSensor::getDmpRate(int64_t *wanted)
+{
+ if (checkDMPOrientation() || checkLPQuaternion()) {
+ // set DMP output rate to FIFO
+ LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)",
+ int(1000000000.f / *wanted), mpu.dmp_output_rate,
+ getTimestamp());
+ write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted);
+ LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *wanted);
+
+ //DMP running rate must be @ 200Hz
+ *wanted= RATE_200HZ;
+ LOGV_IF(PROCESS_VERBOSE, "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted);
+ }
+ return 0;
+}
+
int MPLSensor::getPollTime()
{
VHANDLER_LOG;
@@ -2194,7 +2474,7 @@ int MPLSensor::inv_read_temperature(long long *data)
return -1;
}
- LOGV_IF(ENG_VERBOSE,
+ LOGV_IF(ENG_VERBOSE,
"HAL:temperature raw = %ld, timestamp = %lld, count = %d",
raw, timestamp, count);
data[0] = raw;
@@ -2252,41 +2532,39 @@ int MPLSensor::inv_read_sensor_bias(int fd, long *data)
count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
if(count) {
/* scale appropriately for MPL */
- LOGV_IF(ENG_VERBOSE,
- "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
+ LOGV_IF(ENG_VERBOSE,
+ "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)",
atol(x), atol(y), atol(z));
data[0] = (long)(atol(x) / 10000 * (1L << 16));
data[1] = (long)(atol(y) / 10000 * (1L << 16));
data[2] = (long)(atol(z) / 10000 * (1L << 16));
- LOGV_IF(ENG_VERBOSE,
- "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
+ LOGV_IF(ENG_VERBOSE,
+ "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)",
data[0], data[1], data[2]);
}
return 0;
}
-
/** fill in the sensor list based on which sensors are configured.
* return the number of configured sensors.
* parameter list must point to a memory region of at least 7*sizeof(sensor_t)
* parameter len gives the length of the buffer pointed to by list
*/
-
int MPLSensor::populateSensorList(struct sensor_t *list, int len)
{
VFUNC_LOG;
int numsensors;
- if(len < (int)(7 * sizeof(sensor_t))) {
+ if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) {
LOGE("HAL:sensor list too small, not populating.");
- return 0;
+ return -(sizeof(sSensorList) / sizeof(sensor_t));
}
/* fill in the base values */
- memcpy(list, sSensorList, sizeof (struct sensor_t) * 7);
+ memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t)));
/* first add gyro, accel and compass to the list */
@@ -2301,7 +2579,7 @@ int MPLSensor::populateSensorList(struct sensor_t *list, int len)
mCompassSensor->fillList(&list[MagneticField]);
if(1) {
- numsensors = 7;
+ numsensors = (sizeof(sSensorList) / sizeof(sensor_t));
/* all sensors will be added to the list
fill in orientation values */
fillOrientation(list);
@@ -2335,10 +2613,13 @@ void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
list[Accelerometer].power = ACCEL_MPU6050_POWER;
-
- // TODO: for GED tablet
- // list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
- list[Accelerometer].minDelay = 5000;
+ list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
+ return;
+ } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) {
+ list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE;
+ list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION;
+ list[Accelerometer].power = ACCEL_MPU6500_POWER;
+ list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY;
return;
} else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
@@ -2347,7 +2628,7 @@ void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
list[Accelerometer].power = ACCEL_MPU9150_POWER;
list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
return;
- } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
+ } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) {
list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
list[Accelerometer].power = ACCEL_BMA250_POWER;
@@ -2378,10 +2659,12 @@ void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
list[Gyro].maxRange = GYRO_MPU6050_RANGE;
list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
list[Gyro].power = GYRO_MPU6050_POWER;
-
- // TODO: for GED tablet
- // list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
- list[Gyro].minDelay = 5000;
+ list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
+ } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) {
+ list[Gyro].maxRange = GYRO_MPU6500_RANGE;
+ list[Gyro].resolution = GYRO_MPU6500_RESOLUTION;
+ list[Gyro].power = GYRO_MPU6500_POWER;
+ list[Gyro].minDelay = GYRO_MPU6500_MINDELAY;
} else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
list[Gyro].maxRange = GYRO_MPU9150_RANGE;
list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
@@ -2395,6 +2678,12 @@ void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
list[Gyro].power = GYRO_MPU3050_POWER;
list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
}
+
+ list[RawGyro].maxRange = list[Gyro].maxRange;
+ list[RawGyro].resolution = list[Gyro].resolution;
+ list[RawGyro].power = list[Gyro].power;
+ list[RawGyro].minDelay = list[Gyro].minDelay;
+
return;
}
@@ -2404,14 +2693,11 @@ void MPLSensor::fillRV(struct sensor_t *list)
VFUNC_LOG;
/* compute power on the fly */
- list[RotationVector].power = list[Gyro].power +
+ list[RotationVector].power = list[Gyro].power +
list[Accelerometer].power +
list[MagneticField].power;
list[RotationVector].resolution = .00001;
list[RotationVector].maxRange = 1.0;
-
- // TODO: for GED tablet
- // list[RotationVector].minDelay = 10000;
list[RotationVector].minDelay = 5000;
return;
@@ -2426,9 +2712,6 @@ void MPLSensor::fillOrientation(struct sensor_t *list)
list[MagneticField].power;
list[Orientation].resolution = .00001;
list[Orientation].maxRange = 360.0;
-
- // TODO: for GED tablet
- // list[Orientation].minDelay = 10000;
list[Orientation].minDelay = 5000;
return;
@@ -2443,9 +2726,6 @@ void MPLSensor::fillGravity( struct sensor_t *list)
list[MagneticField].power;
list[Gravity].resolution = .00001;
list[Gravity].maxRange = 9.81;
-
- // TODO: for GED tablet
- // list[Gravity].minDelay = 10000;
list[Gravity].minDelay = 5000;
return;
@@ -2460,9 +2740,6 @@ void MPLSensor::fillLinearAccel(struct sensor_t *list)
list[MagneticField].power;
list[LinearAccel].resolution = list[Accelerometer].resolution;
list[LinearAccel].maxRange = list[Accelerometer].maxRange;
-
- // TODO: for GED tablet
- // list[LinearAccel].minDelay = 10000;
list[LinearAccel].minDelay = 5000;
return;
@@ -2484,7 +2761,7 @@ int MPLSensor::inv_init_sysfs_attributes(void)
ALOGE("MPLSensor failed get sysfs path");
return -1;
}
- sysfs_names_ptr =
+ sysfs_names_ptr =
(char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
sptr = sysfs_names_ptr;
if (sptr != NULL) {
@@ -2512,6 +2789,7 @@ int MPLSensor::inv_init_sysfs_attributes(void)
sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded");
sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on");
+ sprintf(mpu.dmp_event_int_on,"%s%s", sysfs_path, "/dmp_event_int_on");
sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate");
sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");
@@ -2528,18 +2806,15 @@ int MPLSensor::inv_init_sysfs_attributes(void)
#ifdef THIRD_PARTY_ACCEL //BMA250
/* same as 'mpu.accel_enable' */
- sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/enable");
- sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/fifo_rate");
+ sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
+ sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/NA");
sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/NA");
- sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/NA");
+ sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
#else
sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
- sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
- sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
- sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");
// TODO: for bias settings
sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias");
@@ -2547,13 +2822,20 @@ int MPLSensor::inv_init_sysfs_attributes(void)
sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
#endif
+ sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
+ sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
+ sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");
+
sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on");
sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en");
sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en");
sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en");
sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en");
-#if 0
+ sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on");
+ sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation");
+
+#if SYSFS_VERBOSE
// test print sysfs paths
dptr = (char**)&mpu;
for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-26 01:44:51 +0000