diff options
Diffstat (limited to 'libsensors/MPLSensor.cpp')
| -rw-r--r-- | libsensors/MPLSensor.cpp | 1128 |
1 files changed, 1128 insertions, 0 deletions
diff --git a/libsensors/MPLSensor.cpp b/libsensors/MPLSensor.cpp new file mode 100644 index 0000000..ca08ad5 --- /dev/null +++ b/libsensors/MPLSensor.cpp @@ -0,0 +1,1128 @@ +/* + * Copyright (C) 2011 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. + */ + +//#define LOG_NDEBUG 0 +//see also the EXTRA_VERBOSE define, below + +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <float.h> +#include <poll.h> +#include <unistd.h> +#include <dirent.h> +#include <stdlib.h> +#include <sys/select.h> +#include <dlfcn.h> +#include <pthread.h> + +#include <cutils/log.h> +#include <utils/KeyedVector.h> + +#include "MPLSensor.h" + +#include "math.h" +#include "ml.h" +#include "mlFIFO.h" +#include "mlsl.h" +#include "mlos.h" +#include "ml_mputest.h" +#include "ml_stored_data.h" +#include "mldl_cfg.h" +#include "mldl.h" + +#include "mpu.h" +#include "accel.h" +#include "compass.h" +#include "kernel/timerirq.h" +#include "kernel/mpuirq.h" +#include "kernel/slaveirq.h" + +extern "C" { +#include "mlsupervisor.h" +} + +#include "mlcontrol.h" +#include "sensor_params.h" + +#define EXTRA_VERBOSE (0) +//#define FUNC_LOG ALOGV("%s", __PRETTY_FUNCTION__) +#define FUNC_LOG +#define VFUNC_LOG ALOGV_IF(EXTRA_VERBOSE, "%s", __PRETTY_FUNCTION__) +/* this mask must turn on only the sensors that are present and managed by the MPL */ +#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_COMPASS | INV_THREE_AXIS_GYRO) + +#define CALL_MEMBER_FN(pobject,ptrToMember) ((pobject)->*(ptrToMember)) + +/******************************************/ + +/* Base values for the sensor list, these need to be in the order defined in MPLSensor.h */ +static struct sensor_t sSensorList[] = +{ + {"MPL Gyroscope", "Invensense", 1, SENSORS_GYROSCOPE_HANDLE, + SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, + SENSOR_STRING_TYPE_GYROSCOPE, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}}, + {"MPL Accelerometer", "Invensense", 1, SENSORS_ACCELERATION_HANDLE, + SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, + SENSOR_STRING_TYPE_ACCELEROMETER, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}}, + {"MPL Magnetic Field", "Invensense", 1, SENSORS_MAGNETIC_FIELD_HANDLE, + SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, + SENSOR_STRING_TYPE_MAGNETIC_FIELD, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}}, + {"MPL Orientation", "Invensense", 1, SENSORS_ORIENTATION_HANDLE, + SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, + SENSOR_STRING_TYPE_ORIENTATION, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}}, + {"MPL Rotation Vector", "Invensense", 1, SENSORS_ROTATION_VECTOR_HANDLE, + SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, + SENSOR_STRING_TYPE_ORIENTATION, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}}, + {"MPL Linear Acceleration", "Invensense", 1, SENSORS_LINEAR_ACCEL_HANDLE, + SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, + SENSOR_STRING_TYPE_LINEAR_ACCELERATION, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}}, + {"MPL Gravity", "Invensense", 1, SENSORS_GRAVITY_HANDLE, + SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, + SENSOR_STRING_TYPE_GRAVITY, "", 0, SENSOR_FLAG_CONTINUOUS_MODE, {}}, +}; + +static unsigned long long irq_timestamp = 0; +/* *************************************************************************** + * MPL interface misc. + */ +//static pointer to the object that will handle callbacks +static MPLSensor* gMPLSensor = NULL; + +/* we need to pass some callbacks to the MPL. The mpl is a C library, so + * wrappers for the C++ callback implementations are required. + */ +extern "C" { +//callback wrappers go here +void mot_cb_wrapper(uint16_t val) +{ + if (gMPLSensor) { + gMPLSensor->cbOnMotion(val); + } +} + +void procData_cb_wrapper() +{ + if (gMPLSensor) { + gMPLSensor->cbProcData(); + } +} + +} //end of extern C + +void setCallbackObject(MPLSensor* gbpt) +{ + gMPLSensor = gbpt; +} + + +/***************************************************************************** + * sensor class implementation + */ + +#define GY_ENABLED ((1<<ID_GY) & enabled_sensors) +#define A_ENABLED ((1<<ID_A) & enabled_sensors) +#define O_ENABLED ((1<<ID_O) & enabled_sensors) +#define M_ENABLED ((1<<ID_M) & 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) + +MPLSensor::MPLSensor() : + SensorBase(NULL, NULL), + mNewData(0), + mDmpStarted(false), + mMasterSensorMask(INV_ALL_SENSORS), + mLocalSensorMask(ALL_MPL_SENSORS_NP), + mCurFifoRate(-1), mHaveGoodMpuCal(false), mHaveGoodCompassCal(false), + mUseTimerIrqAccel(false), mUsetimerIrqCompass(true), + mUseTimerirq(false), + mEnabled(0), mPendingMask(0) +{ + FUNC_LOG; + int mpu_int_fd; + char *port = NULL; + + ALOGV_IF(EXTRA_VERBOSE, "MPLSensor constructor: numSensors = %d", numSensors); + + pthread_mutex_init(&mMplMutex, NULL); + + mForceSleep = false; + + /* used for identifying whether 9axis is enabled or not */ + /* this variable will be changed in initMPL() when libmpl is loaded */ + /* sensor list will be changed based on this variable */ + mNineAxisEnabled = false; + + for (size_t i = 0; i < ARRAY_SIZE(mPollFds); i++) { + mPollFds[i].fd = -1; + mPollFds[i].events = 0; + } + + pthread_mutex_lock(&mMplMutex); + + mpu_int_fd = open("/dev/mpuirq", O_RDWR); + if (mpu_int_fd == -1) { + ALOGE("could not open the mpu irq device node"); + } else { + fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK); + mIrqFds.add(MPUIRQ_FD, mpu_int_fd); + mPollFds[MPUIRQ_FD].fd = mpu_int_fd; + mPollFds[MPUIRQ_FD].events = POLLIN; + } + + accel_fd = open("/dev/accelirq", O_RDWR); + if (accel_fd == -1) { + ALOGE("could not open the accel irq device node"); + } else { + fcntl(accel_fd, F_SETFL, O_NONBLOCK); + mIrqFds.add(ACCELIRQ_FD, accel_fd); + mPollFds[ACCELIRQ_FD].fd = accel_fd; + mPollFds[ACCELIRQ_FD].events = POLLIN; + } + + timer_fd = open("/dev/timerirq", O_RDWR); + if (timer_fd == -1) { + ALOGE("could not open the timer irq device node"); + } else { + fcntl(timer_fd, F_SETFL, O_NONBLOCK); + mIrqFds.add(TIMERIRQ_FD, timer_fd); + mPollFds[TIMERIRQ_FD].fd = timer_fd; + mPollFds[TIMERIRQ_FD].events = POLLIN; + } + + data_fd = mpu_int_fd; + + if ((accel_fd == -1) && (timer_fd != -1)) { + //no accel irq and timer available + mUseTimerIrqAccel = true; + } + + memset(mPendingEvents, 0, sizeof(mPendingEvents)); + + mPendingEvents[RotationVector].version = sizeof(sensors_event_t); + mPendingEvents[RotationVector].sensor = ID_RV; + mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; + + mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); + mPendingEvents[LinearAccel].sensor = ID_LA; + mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; + + mPendingEvents[Gravity].version = sizeof(sensors_event_t); + mPendingEvents[Gravity].sensor = ID_GR; + mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; + + mPendingEvents[Gyro].version = sizeof(sensors_event_t); + mPendingEvents[Gyro].sensor = ID_GY; + mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; + + mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); + mPendingEvents[Accelerometer].sensor = ID_A; + mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; + + mPendingEvents[MagneticField].version = sizeof(sensors_event_t); + mPendingEvents[MagneticField].sensor = ID_M; + mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; + mPendingEvents[MagneticField].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[Orientation].version = sizeof(sensors_event_t); + mPendingEvents[Orientation].sensor = ID_O; + mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; + mPendingEvents[Orientation].orientation.status + = SENSOR_STATUS_ACCURACY_HIGH; + + mHandlers[RotationVector] = &MPLSensor::rvHandler; + mHandlers[LinearAccel] = &MPLSensor::laHandler; + mHandlers[Gravity] = &MPLSensor::gravHandler; + mHandlers[Gyro] = &MPLSensor::gyroHandler; + mHandlers[Accelerometer] = &MPLSensor::accelHandler; + mHandlers[MagneticField] = &MPLSensor::compassHandler; + mHandlers[Orientation] = &MPLSensor::orienHandler; + + for (int i = 0; i < numSensors; i++) + mDelays[i] = 30000000LLU; // 30 ms by default + + if (inv_serial_start(port) != INV_SUCCESS) { + ALOGE("Fatal Error : could not open MPL serial interface"); + } + + //initialize library parameters + initMPL(); + + //setup the FIFO contents + setupFIFO(); + + + pthread_mutex_unlock(&mMplMutex); +} + +MPLSensor::~MPLSensor() +{ + FUNC_LOG; + pthread_mutex_lock(&mMplMutex); + if (inv_dmp_stop() != INV_SUCCESS) { + ALOGW("Error: could not stop the DMP correctly.\n"); + } + + if (inv_dmp_close() != INV_SUCCESS) { + ALOGW("Error: could not close the DMP"); + } + + if (inv_serial_stop() != INV_SUCCESS) { + ALOGW("Error : could not close the serial port"); + } + pthread_mutex_unlock(&mMplMutex); + pthread_mutex_destroy(&mMplMutex); +} + +/* clear any data from our various filehandles */ +void MPLSensor::clearIrqData(bool* irq_set) +{ + unsigned int i; + int nread; + struct mpuirq_data irqdata; + + poll(mPollFds, ARRAY_SIZE(mPollFds), 0); //check which ones need to be cleared + + for (i = 0; i < ARRAY_SIZE(mPollFds); i++) { + int cur_fd = mPollFds[i].fd; + if (mPollFds[i].revents & POLLIN) { + nread = read(cur_fd, &irqdata, sizeof(irqdata)); + if (nread > 0) { + irq_set[i] = true; + irq_timestamp = irqdata.irqtime; + } + } + mPollFds[i].revents = 0; + } +} + +/* set the power states of the various sensors based on the bits set in the + * enabled_sensors parameter. + * this function modifies globalish state variables. It must be called with the mMplMutex held. */ +void MPLSensor::setPowerStates(int enabled_sensors) +{ + FUNC_LOG; + bool irq_set[5] = { false, false, false, false, false }; + + do { + if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { + mLocalSensorMask = ALL_MPL_SENSORS_NP; + break; + } + + if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) { + mLocalSensorMask = 0; + break; + } + + if (GY_ENABLED) { + mLocalSensorMask |= INV_THREE_AXIS_GYRO; + } else { + mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; + } + + if (A_ENABLED) { + mLocalSensorMask |= (INV_THREE_AXIS_ACCEL); + } else { + mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL); + } + + if (M_ENABLED) { + mLocalSensorMask |= INV_THREE_AXIS_COMPASS; + } else { + mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS); + } + } while (0); + + //record the new sensor state + inv_error_t rv; + + unsigned long sen_mask = mLocalSensorMask & mMasterSensorMask; + + bool changing_sensors = ((inv_get_dl_config()->requested_sensors + != sen_mask) && (sen_mask != 0)); + bool restart = (!mDmpStarted) && (sen_mask != 0); + + if (changing_sensors || restart) { + + ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart); + + if (mDmpStarted) { + inv_dmp_stop(); + clearIrqData(irq_set); + mDmpStarted = false; + } + + if (sen_mask != inv_get_dl_config()->requested_sensors) { + rv = inv_set_mpu_sensors(sen_mask); + ALOGE_IF(rv != INV_SUCCESS, + "error: unable to set MPL sensor power states (sens=%ld retcode = %d)", + sen_mask, rv); + } + + if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS)) + || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL))) + && ((sen_mask & INV_DMP_PROCESSOR) == 0)) { + ALOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ"); + mUseTimerirq = true; + } else { + if (mUseTimerirq) { + ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); + clearIrqData(irq_set); + } + ALOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ"); + mUseTimerirq = false; + } + + if (!mDmpStarted) { + if (mHaveGoodMpuCal || mHaveGoodCompassCal) { + rv = inv_store_calibration(); + ALOGE_IF(rv != INV_SUCCESS, + "error: unable to store MPL calibration file"); + mHaveGoodMpuCal = false; + mHaveGoodCompassCal = false; + } + rv = inv_dmp_start(); + ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp"); + mDmpStarted = true; + } + } + + //check if we should stop the DMP + if (mDmpStarted && (sen_mask == 0)) { + rv = inv_dmp_stop(); + ALOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)", + rv); + if (mUseTimerirq) { + ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); + } + clearIrqData(irq_set); + + mDmpStarted = false; + mCurFifoRate = -1; + } +} + +/** + * container function for all the calls we make once to set up the MPL. + */ +void MPLSensor::initMPL() +{ + FUNC_LOG; + inv_error_t result; + unsigned short bias_update_mask = 0xFFFF; + + if (inv_dmp_open() != INV_SUCCESS) { + ALOGE("Fatal Error : could not open DMP correctly.\n"); + } + + result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work + ALOGE_IF(result != INV_SUCCESS, + "Fatal Error : could not set enabled sensors."); + + if (inv_load_calibration() != INV_SUCCESS) { + ALOGE("could not open MPL calibration file"); + } + + //check for the 9axis fusion library: if available load it and start 9x + void* h_dmp_lib=dlopen("libinvensense_mpl.so", RTLD_NOW); + if(h_dmp_lib) { + const char* error; + error = dlerror(); + inv_error_t (*fp_inv_enable_9x_fusion)() = + (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion"); + if((error = dlerror()) != NULL) { + ALOGE("%s %s", error, "inv_enable_9x_fusion"); + } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) { + ALOGE( "Warning : 9 axis sensor fusion not available " + "- No compass detected.\n"); + } else { + /* 9axis is loaded and enabled */ + /* this variable is used for coming up with sensor list */ + mNineAxisEnabled = true; + } + } else { + const char* error = dlerror(); + ALOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error); + } + + if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) { + ALOGE("Error : Bias update function could not be set.\n"); + } + + if (inv_set_motion_interrupt(1) != INV_SUCCESS) { + ALOGE("Error : could not set motion interrupt"); + } + + if (inv_set_fifo_interrupt(1) != INV_SUCCESS) { + ALOGE("Error : could not set fifo interrupt"); + } + + result = inv_set_fifo_rate(6); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result); + } + + setupCallbacks(); +} + +/** setup the fifo contents. + */ +void MPLSensor::setupFIFO() +{ + FUNC_LOG; + inv_error_t result; + + result = inv_send_accel(INV_ALL, INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_accel returned %d\n", result); + } + + result = inv_send_quaternion(INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_quaternion returned %d\n", result); + } + + result = inv_send_linear_accel(INV_ALL, INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_linear_accel returned %d\n", result); + } + + result = inv_send_linear_accel_in_world(INV_ALL, INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_linear_accel_in_world returned %d\n", + result); + } + + result = inv_send_gravity(INV_ALL, INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_gravity returned %d\n", result); + } + + result = inv_send_gyro(INV_ALL, INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_gyro returned %d\n", result); + } +} + +/** + * set up the callbacks that we use in all cases (outside of gestures, etc) + */ +void MPLSensor::setupCallbacks() +{ + FUNC_LOG; + if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) { + ALOGE("Error : Motion callback could not be set.\n"); + + } + + if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) { + ALOGE("Error : Processed data callback could not be set."); + + } +} + +/** + * handle the motion/no motion output from the MPL. + */ +void MPLSensor::cbOnMotion(uint16_t val) +{ + FUNC_LOG; + //after the first no motion, the gyro should be calibrated well + if (val == 2) { + if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) { + //if gyros are on and we got a no motion, set a flag + // indicating that the cal file can be written. + mHaveGoodMpuCal = true; + } + } + + return; +} + + +void MPLSensor::cbProcData() +{ + mNewData = 1; +} + +//these handlers transform mpl data into one of the Android sensor types +// scaling and coordinate transforms should be done in the handlers + +void MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + VFUNC_LOG; + inv_error_t res; + res = inv_get_float_array(INV_GYROS, s->gyro.v); + s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0; + s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0; + s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0; + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); +} + +void MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + //VFUNC_LOG; + inv_error_t res; + res = inv_get_float_array(INV_ACCELS, s->acceleration.v); + //res = inv_get_accel_float(s->acceleration.v); + s->acceleration.v[0] = s->acceleration.v[0] * 9.81; + s->acceleration.v[1] = s->acceleration.v[1] * 9.81; + s->acceleration.v[2] = s->acceleration.v[2] * 9.81; + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); +} + +int MPLSensor::estimateCompassAccuracy() +{ + inv_error_t res; + int rv; + + res = inv_get_compass_accuracy(&rv); + if(rv >= SENSOR_STATUS_ACCURACY_MEDIUM) { + mHaveGoodCompassCal = true; + } + ALOGE_IF(res != INV_SUCCESS, "error returned from inv_get_compass_accuracy"); + + return rv; +} + +void MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + VFUNC_LOG; + inv_error_t res; + + res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v); + + if (res != INV_SUCCESS) { + ALOGW( + "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d", + res); + } + + s->magnetic.status = estimateCompassAccuracy(); + + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); +} + +void MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + VFUNC_LOG; + float quat[4]; + float norm = 0; + inv_error_t r; + + r = inv_get_float_array(INV_QUATERNION, quat); + + if (r != INV_SUCCESS) { + *pending_mask &= ~(1 << index); + return; + } else { + *pending_mask |= (1 << index); + } + + norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3] + + FLT_EPSILON; + + if (norm > 1.0f) { + //renormalize + norm = sqrtf(norm); + float inv_norm = 1.0f / norm; + quat[1] = quat[1] * inv_norm; + quat[2] = quat[2] * inv_norm; + quat[3] = quat[3] * inv_norm; + } + + if (quat[0] < 0.0) { + quat[1] = -quat[1]; + quat[2] = -quat[2]; + quat[3] = -quat[3]; + } + + s->gyro.v[0] = quat[1]; + s->gyro.v[1] = quat[2]; + s->gyro.v[2] = quat[3]; +} + +void MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + VFUNC_LOG; + inv_error_t res; + res = inv_get_float_array(INV_LINEAR_ACCELERATION, s->gyro.v); + s->gyro.v[0] *= 9.81; + s->gyro.v[1] *= 9.81; + s->gyro.v[2] *= 9.81; + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); +} + +void MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + VFUNC_LOG; + inv_error_t res; + res = inv_get_float_array(INV_GRAVITY, s->gyro.v); + s->gyro.v[0] *= 9.81; + s->gyro.v[1] *= 9.81; + s->gyro.v[2] *= 9.81; + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); +} + +void MPLSensor::calcOrientationSensor(float *R, float *values) +{ + float tmp; + + //Azimuth + if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) { + values[0] = (float) atan2f(-R[3], R[0]); + } else { + values[0] = (float) atan2f(R[1], R[4]); + } + values[0] *= 57.295779513082320876798154814105f; + if (values[0] < 0) { + values[0] += 360.0f; + } + //Pitch + tmp = R[7]; + if (tmp > 1.0f) + tmp = 1.0f; + if (tmp < -1.0f) + tmp = -1.0f; + values[1] = -asinf(tmp) * 57.295779513082320876798154814105f; + if (R[8] < 0) { + values[1] = 180.0f - values[1]; + } + if (values[1] > 180.0f) { + values[1] -= 360.0f; + } + //Roll + if ((R[7] > 0.7071067f)) { + values[2] = (float) atan2f(R[6], R[7]); + } else { + values[2] = (float) atan2f(R[6], R[8]); + } + + values[2] *= 57.295779513082320876798154814105f; + if (values[2] > 90.0f) { + values[2] = 180.0f - values[2]; + } + if (values[2] < -90.0f) { + values[2] = -180.0f - values[2]; + } +} + +void MPLSensor::orienHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) //note that this is the handler for the android 'orientation' sensor, not the mpl orientation output +{ + VFUNC_LOG; + inv_error_t res; + float rot_mat[9]; + + res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat); + + calcOrientationSensor(rot_mat, s->orientation.v); + + s->orientation.status = estimateCompassAccuracy(); + + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); + else + ALOGW("orienHandler: data not valid (%d)", (int) res); +} + +int MPLSensor::enable(int32_t handle, int en) +{ + FUNC_LOG; + + int what = -1; + + switch (handle) { + case ID_A: + what = Accelerometer; + break; + case ID_M: + what = MagneticField; + break; + case ID_O: + what = Orientation; + break; + case ID_GY: + what = Gyro; + break; + case ID_GR: + what = Gravity; + break; + case ID_RV: + what = RotationVector; + break; + case ID_LA: + what = LinearAccel; + break; + default: //this takes care of all the gestures + what = handle; + break; + } + + if (uint32_t(what) >= numSensors) + return -EINVAL; + + int newState = en ? 1 : 0; + int err = 0; + + pthread_mutex_lock(&mMplMutex); + if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { + short flags = newState; + mEnabled &= ~(1 << what); + mEnabled |= (uint32_t(flags) << what); + ALOGV_IF(EXTRA_VERBOSE, "mEnabled = %x", mEnabled); + setPowerStates(mEnabled); + pthread_mutex_unlock(&mMplMutex); + if (!newState) + update_delay(); + return err; + } + pthread_mutex_unlock(&mMplMutex); + return err; +} + +int MPLSensor::setDelay(int32_t handle, int64_t ns) +{ + FUNC_LOG; + ALOGV_IF(EXTRA_VERBOSE, + " setDelay handle: %d rate %d", handle, (int) (ns / 1000000LL)); + int what = -1; + switch (handle) { + case ID_A: + what = Accelerometer; + break; + case ID_M: + what = MagneticField; + break; + case ID_O: + what = Orientation; + break; + case ID_GY: + what = Gyro; + break; + case ID_GR: + what = Gravity; + break; + case ID_RV: + what = RotationVector; + break; + case ID_LA: + what = LinearAccel; + break; + default: + what = handle; + break; + } + + if (uint32_t(what) >= numSensors) + return -EINVAL; + + if (ns < 0) + return -EINVAL; + + pthread_mutex_lock(&mMplMutex); + mDelays[what] = ns; + pthread_mutex_unlock(&mMplMutex); + return update_delay(); +} + +int MPLSensor::update_delay() +{ + FUNC_LOG; + int rv = 0; + bool irq_set[5]; + + pthread_mutex_lock(&mMplMutex); + + if (mEnabled) { + uint64_t wanted = -1LLU; + for (int i = 0; i < numSensors; i++) { + if (mEnabled & (1 << i)) { + uint64_t ns = mDelays[i]; + wanted = wanted < ns ? wanted : ns; + } + } + + //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns + if (wanted < 10000000LLU) { + wanted = 10000000LLU; + } + + int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1 + : 0); //mpu fifo rate is in increments of 5ms + if (rate == 0) //KLP disallow fifo rate 0 + rate = 1; + + if (rate != mCurFifoRate) { + inv_error_t res; // = inv_dmp_stop(); + res = inv_set_fifo_rate(rate); + ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate"); + + mCurFifoRate = rate; + rv = (res == INV_SUCCESS); + } + + if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) { + if (mUseTimerirq) { + ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); + clearIrqData(irq_set); + if (inv_get_dl_config()->requested_sensors + == INV_THREE_AXIS_COMPASS) { + ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, + (unsigned long) (wanted / 1000000LLU)); + ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", + (int) (wanted / 1000000LLU)); + } else { + ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, + (unsigned long) inv_get_sample_step_size_ms()); + ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", + (int) inv_get_sample_step_size_ms()); + } + } + } + + } + pthread_mutex_unlock(&mMplMutex); + return rv; +} + +int MPLSensor::readEvents(sensors_event_t* data, int count) +{ + //VFUNC_LOG; + bool irq_set[5] = { false, false, false, false, false }; + inv_error_t rv; + if (count < 1) + return -EINVAL; + int numEventReceived = 0; + + clearIrqData(irq_set); + + pthread_mutex_lock(&mMplMutex); + if (mDmpStarted) { + rv = inv_update_data(); + ALOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv); + } + + else { + //probably just one extra read after shutting down + ALOGV_IF(EXTRA_VERBOSE, + "MPLSensor::readEvents called, but there's nothing to do."); + } + + pthread_mutex_unlock(&mMplMutex); + + if (!mNewData) { + ALOGV_IF(EXTRA_VERBOSE, "no new data"); + return 0; + } + mNewData = 0; + + /* google timestamp */ + pthread_mutex_lock(&mMplMutex); + for (int i = 0; i < numSensors; i++) { + if (mEnabled & (1 << i)) { + CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i, + &mPendingMask, i); + mPendingEvents[i].timestamp = irq_timestamp; + } + } + + for (int j = 0; count && mPendingMask && j < numSensors; j++) { + if (mPendingMask & (1 << j)) { + mPendingMask &= ~(1 << j); + if (mEnabled & (1 << j)) { + *data++ = mPendingEvents[j]; + count--; + numEventReceived++; + } + } + } + + pthread_mutex_unlock(&mMplMutex); + return numEventReceived; +} + +int MPLSensor::getFd() const +{ + return data_fd; +} + +int MPLSensor::getAccelFd() const +{ + return accel_fd; +} + +int MPLSensor::getTimerFd() const +{ + return timer_fd; +} + +int MPLSensor::getPowerFd() const +{ + return (uintptr_t) inv_get_serial_handle(); +} + +void MPLSensor::handlePowerEvent() +{ + VFUNC_LOG; + mpuirq_data irqd; + + int fd = (uintptr_t) inv_get_serial_handle(); + read(fd, &irqd, sizeof(irqd)); + + if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) { + //going to sleep + sleepEvent(); + } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) { + //waking up + wakeEvent(); + } + + ioctl(fd, MPU_PM_EVENT_HANDLED, 0); +} + +void MPLSensor::sleepEvent() +{ + VFUNC_LOG; + pthread_mutex_lock(&mMplMutex); + if (mEnabled != 0) { + mForceSleep = true; + mOldEnabledMask = mEnabled; + setPowerStates(0); + } + pthread_mutex_unlock(&mMplMutex); +} + +void MPLSensor::wakeEvent() +{ + VFUNC_LOG; + pthread_mutex_lock(&mMplMutex); + if (mForceSleep) { + setPowerStates((mOldEnabledMask | mEnabled)); + } + mForceSleep = false; + pthread_mutex_unlock(&mMplMutex); +} + +/** 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, size_t len) +{ + int numsensors; + + if(len < 7*sizeof(sensor_t)) { + ALOGE("sensor list too small, not populating."); + return 0; + } + + /* fill in the base values */ + memcpy(list, sSensorList, sizeof (struct sensor_t) * 7); + + /* first add gyro, accel and compass to the list */ + + /* fill in accel values */ + list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; + list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; + list[Accelerometer].power = ACCEL_BMA250_POWER; + + /* fill in compass values */ + list[MagneticField].maxRange = COMPASS_YAS530_RANGE; + list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION; + list[MagneticField].power = COMPASS_YAS530_POWER; + + /* fill in gyro values */ + list[Gyro].maxRange = GYRO_MPU3050_RANGE; + list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; + list[Gyro].power = GYRO_MPU3050_POWER; + + if(mNineAxisEnabled) + { + numsensors = 7; + /* all sensors will be added to the list */ + /* fill in orientation values */ + fillOrientation(list); + + /* fill in rotation vector values */ + fillRV(list); + + /* fill in gravity values */ + fillGravity(list); + + /* fill in Linear accel values */ + fillLinearAccel(list); + } else { + /* no 9-axis sensors, zero fill that part of the list */ + numsensors = 3; + memset(list+3, 0, 4*sizeof(struct sensor_t)); + } + + return numsensors; +} + +/* fillRV depends on values of accel and compass in the list */ +void MPLSensor::fillRV(struct sensor_t *list) +{ + /* compute power on the fly */ + list[RotationVector].power = list[Gyro].power + list[Accelerometer].power + + list[MagneticField].power; + list[RotationVector].resolution = .00001; + list[RotationVector].maxRange = 1.0; + return; +} + +void MPLSensor::fillOrientation(struct sensor_t *list) +{ + list[Orientation].power = list[Gyro].power + list[Accelerometer].power + + list[MagneticField].power; + list[Orientation].resolution = .00001; + list[Orientation].maxRange = 360.0; + return; +} + +void MPLSensor::fillGravity( struct sensor_t *list) +{ + list[Gravity].power = list[Gyro].power + list[Accelerometer].power + + list[MagneticField].power; + list[Gravity].resolution = .00001; + list[Gravity].maxRange = 9.81; + return; +} + +void MPLSensor::fillLinearAccel(struct sensor_t *list) +{ + list[Gravity].power = list[Gyro].power + list[Accelerometer].power + + list[MagneticField].power; + list[Gravity].resolution = list[Accelerometer].resolution; + list[Gravity].maxRange = list[Accelerometer].maxRange; + return; +} |