/*
* 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 LOGV("%s", __PRETTY_FUNCTION__)
#define FUNC_LOG
#define VFUNC_LOG LOGV_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, { } },
{ "MPL Accelerometer", "Invensense", 1,
SENSORS_ACCELERATION_HANDLE,
SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, { } },
{ "MPL Magnetic Field", "Invensense", 1,
SENSORS_MAGNETIC_FIELD_HANDLE,
SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, { } },
{ "MPL Orientation", "Invensense", 1,
SENSORS_ORIENTATION_HANDLE,
SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, { } },
{ "MPL Rotation Vector", "Invensense", 1,
SENSORS_ROTATION_VECTOR_HANDLE,
SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, { } },
{ "MPL Linear Acceleration", "Invensense", 1,
SENSORS_LINEAR_ACCEL_HANDLE,
SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, { } },
{ "MPL Gravity", "Invensense", 1,
SENSORS_GRAVITY_HANDLE,
SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, { } },
};
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),
mMpuAccuracy(0), mNewData(0),
mDmpStarted(false),
mMasterSensorMask(INV_ALL_SENSORS),
mLocalSensorMask(ALL_MPL_SENSORS_NP), mPollTime(-1),
mCurFifoRate(-1), mHaveGoodMpuCal(false), mHaveGoodCompassCal(false),
mUseTimerIrqAccel(false), mUsetimerIrqCompass(true),
mUseTimerirq(false), mSampleCount(0),
mEnabled(0), mPendingMask(0)
{
FUNC_LOG;
inv_error_t rv;
int mpu_int_fd, i;
char *port = NULL;
LOGV_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 (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) {
LOGE("could not open the mpu irq device node");
} else {
fcntl(mpu_int_fd, F_SETFL, O_NONBLOCK);
//ioctl(mpu_int_fd, MPUIRQ_SET_TIMEOUT, 0);
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) {
LOGE("could not open the accel irq device node");
} else {
fcntl(accel_fd, F_SETFL, O_NONBLOCK);
//ioctl(accel_fd, SLAVEIRQ_SET_TIMEOUT, 0);
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) {
LOGE("could not open the timer irq device node");
} else {
fcntl(timer_fd, F_SETFL, O_NONBLOCK);
//ioctl(timer_fd, TIMERIRQ_SET_TIMEOUT, 0);
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;
//LOGD("MPLSensor falling back to timerirq for accel data");
}
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[RotationVector].acceleration.status
= SENSOR_STATUS_ACCURACY_HIGH;
mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
mPendingEvents[LinearAccel].sensor = ID_LA;
mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
mPendingEvents[LinearAccel].acceleration.status
= SENSOR_STATUS_ACCURACY_HIGH;
mPendingEvents[Gravity].version = sizeof(sensors_event_t);
mPendingEvents[Gravity].sensor = ID_GR;
mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
mPendingEvents[Gyro].version = sizeof(sensors_event_t);
mPendingEvents[Gyro].sensor = ID_GY;
mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
mPendingEvents[Accelerometer].sensor = ID_A;
mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
mPendingEvents[Accelerometer].acceleration.status
= SENSOR_STATUS_ACCURACY_HIGH;
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) {
LOGE("Fatal Error : could not open MPL serial interface");
}
//initialize library parameters
initMPL();
//setup the FIFO contents
setupFIFO();
//we start the motion processing only when a sensor is enabled...
//rv = inv_dmp_start();
//LOGE_IF(rv != INV_SUCCESS, "Fatal error: could not start the DMP correctly. (code = %d)\n", rv);
//dmp_started = true;
pthread_mutex_unlock(&mMplMutex);
}
MPLSensor::~MPLSensor()
{
FUNC_LOG;
pthread_mutex_lock(&mMplMutex);
if (inv_dmp_stop() != INV_SUCCESS) {
LOGW("Error: could not stop the DMP correctly.\n");
}
if (inv_dmp_close() != INV_SUCCESS) {
LOGW("Error: could not close the DMP");
}
if (inv_serial_stop() != INV_SUCCESS) {
LOGW("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;
int j = 0;
if (mPollFds[i].revents & POLLIN) {
nread = read(cur_fd, &irqdata, sizeof(irqdata));
if (nread > 0) {
irq_set[i] = true;
irq_timestamp = irqdata.irqtime;
//LOGV_IF(EXTRA_VERBOSE, "irq: %d %d (%d)", i, irqdata.interruptcount, j++);
}
}
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 };
//LOGV(" setPowerStates: %d dmp_started: %d", enabled_sensors, mDmpStarted);
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;
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) {
LOGV_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) {
//LOGV("setPowerStates: %lx", sen_mask);
rv = inv_set_mpu_sensors(sen_mask);
LOGE_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)) {
LOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ");
mUseTimerirq = true;
} else {
if (mUseTimerirq) {
ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
clearIrqData(irq_set);
}
LOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ");
mUseTimerirq = false;
}
if (!mDmpStarted) {
if (mHaveGoodMpuCal || mHaveGoodCompassCal) {
rv = inv_store_calibration();
LOGE_IF(rv != INV_SUCCESS,
"error: unable to store MPL calibration file");
mHaveGoodMpuCal = false;
mHaveGoodCompassCal = false;
}
//LOGV("Starting DMP");
rv = inv_dmp_start();
LOGE_IF(rv != INV_SUCCESS, "unable to start dmp");
mDmpStarted = true;
}
}
//check if we should stop the DMP
if (mDmpStarted && (sen_mask == 0)) {
//LOGV("Stopping DMP");
rv = inv_dmp_stop();
LOGE_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;
mPollTime = -1;
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;
struct mldl_cfg *mldl_cfg;
if (inv_dmp_open() != INV_SUCCESS) {
LOGE("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
LOGE_IF(result != INV_SUCCESS,
"Fatal Error : could not set enabled sensors.");
if (inv_load_calibration() != INV_SUCCESS) {
LOGE("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) {
LOGE("%s %s", error, "inv_enable_9x_fusion");
} else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) {
LOGE( "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();
LOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error);
}
mldl_cfg = inv_get_dl_config();
if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) {
LOGE("Error : Bias update function could not be set.\n");
}
if (inv_set_motion_interrupt(1) != INV_SUCCESS) {
LOGE("Error : could not set motion interrupt");
}
if (inv_set_fifo_interrupt(1) != INV_SUCCESS) {
LOGE("Error : could not set fifo interrupt");
}
result = inv_set_fifo_rate(6);
if (result != INV_SUCCESS) {
LOGE("Fatal error: inv_set_fifo_rate returned %d\n", result);
}
mMpuAccuracy = SENSOR_STATUS_ACCURACY_MEDIUM;
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) {
LOGE("Fatal error: inv_send_accel returned %d\n", result);
}
result = inv_send_quaternion(INV_32_BIT);
if (result != INV_SUCCESS) {
LOGE("Fatal error: inv_send_quaternion returned %d\n", result);
}
result = inv_send_linear_accel(INV_ALL, INV_32_BIT);
if (result != INV_SUCCESS) {
LOGE("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) {
LOGE("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) {
LOGE("Fatal error: inv_send_gravity returned %d\n", result);
}
result = inv_send_gyro(INV_ALL, INV_32_BIT);
if (result != INV_SUCCESS) {
LOGE("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) {
LOGE("Error : Motion callback could not be set.\n");
}
if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) {
LOGE("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) {
mMpuAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
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;
mSampleCount++;
//LOGV_IF(EXTRA_VERBOSE, "new data (%d)", sampleCount);
}
//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;
s->gyro.status = mMpuAccuracy;
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;
//LOGV_IF(EXTRA_VERBOSE, "accel data: %f %f %f", s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2]);
s->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
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;
}
LOGE_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, res2;
float bias_error[3];
float total_be;
static int bias_error_settled = 0;
res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v);
if (res != INV_SUCCESS) {
LOGW(
"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;
float ang = 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];
s->gyro.status
= ((mMpuAccuracy < estimateCompassAccuracy()) ? mMpuAccuracy
: estimateCompassAccuracy());
}
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;
s->gyro.status = mMpuAccuracy;
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;
s->gyro.status = mMpuAccuracy;
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 euler[3];
float heading[1];
float rot_mat[9];
res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat);
//ComputeAndOrientation(heading[0], euler, s->orientation.v);
calcOrientationSensor(rot_mat, s->orientation.v);
s->orientation.status = estimateCompassAccuracy();
if (res == INV_SUCCESS)
*pending_mask |= (1 << index);
else
LOGW("orienHandler: data not valid (%d)", (int) res);
}
int MPLSensor::enable(int32_t handle, int en)
{
FUNC_LOG;
//LOGV("handle : %d en: %d", handle, en);
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;
//LOGV_IF((uint32_t(newState) << what) != (mEnabled & (1 << what)),
// "sensor state change what=%d", what);
pthread_mutex_lock(&mMplMutex);
if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
uint32_t sensor_type;
short flags = newState;
mEnabled &= ~(1 << what);
mEnabled |= (uint32_t(flags) << what);
LOGV_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;
LOGV_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) {
//LOGD("set fifo rate: %d %llu", rate, wanted);
inv_error_t res; // = inv_dmp_stop();
res = inv_set_fifo_rate(rate);
LOGE_IF(res != INV_SUCCESS, "error setting FIFO rate");
//res = inv_dmp_start();
//LOGE_IF(res != INV_SUCCESS, "error re-starting DMP");
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));
LOGV_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());
LOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d",
(int) inv_get_sample_step_size_ms());
}
}
}
}
pthread_mutex_unlock(&mMplMutex);
return rv;
}
/* return the current time in nanoseconds */
int64_t MPLSensor::now_ns(void)
{
//FUNC_LOG;
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
//LOGV("Time %lld", (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec);
return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec;
}
int MPLSensor::readEvents(sensors_event_t* data, int count)
{
//VFUNC_LOG;
int i;
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) {
//LOGV_IF(EXTRA_VERBOSE, "Update Data");
rv = inv_update_data();
LOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv);
}
else {
//probably just one extra read after shutting down
LOGV_IF(EXTRA_VERBOSE,
"MPLSensor::readEvents called, but there's nothing to do.");
}
pthread_mutex_unlock(&mMplMutex);
if (!mNewData) {
LOGV_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
{
//LOGV("MPLSensor::getFd returning %d", data_fd);
return data_fd;
}
int MPLSensor::getAccelFd() const
{
//LOGV("MPLSensor::getAccelFd returning %d", accel_fd);
return accel_fd;
}
int MPLSensor::getTimerFd() const
{
//LOGV("MPLSensor::getTimerFd returning %d", timer_fd);
return timer_fd;
}
int MPLSensor::getPowerFd() const
{
int hdl = (int) inv_get_serial_handle();
//LOGV("MPLSensor::getPowerFd returning %d", hdl);
return hdl;
}
int MPLSensor::getPollTime()
{
return mPollTime;
}
bool MPLSensor::hasPendingEvents() const
{
//if we are using the polling workaround, force the main loop to check for data every time
return (mPollTime != -1);
}
void MPLSensor::handlePowerEvent()
{
VFUNC_LOG;
mpuirq_data irqd;
int fd = (int) 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, int len)
{
int numsensors;
if(len < 7*sizeof(sensor_t)) {
LOGE("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 */
unsigned short accelId = inv_get_accel_id();
if(accelId == 0)
{
LOGE("Can not get accel id");
}
fillAccel(accelId, list);
/* fill in compass values */
unsigned short compassId = inv_get_compass_id();
if(compassId == 0)
{
LOGE("Can not get compass id");
}
fillCompass(compassId, list);
/* fill in gyro values */
fillGyro(MPU_NAME, list);
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;
}
void MPLSensor::fillAccel(unsigned char accel, struct sensor_t *list)
{
switch (accel) {
case ACCEL_ID_LIS331:
list[Accelerometer].maxRange = ACCEL_LIS331_RANGE;
list[Accelerometer].resolution = ACCEL_LIS331_RESOLUTION;
list[Accelerometer].power = ACCEL_LIS331_POWER;
break;
case ACCEL_ID_LIS3DH:
list[Accelerometer].maxRange = ACCEL_LIS3DH_RANGE;
list[Accelerometer].resolution = ACCEL_LIS3DH_RESOLUTION;
list[Accelerometer].power = ACCEL_LIS3DH_POWER;
break;
case ACCEL_ID_KXSD9:
list[Accelerometer].maxRange = ACCEL_KXSD9_RANGE;
list[Accelerometer].resolution = ACCEL_KXSD9_RESOLUTION;
list[Accelerometer].power = ACCEL_KXSD9_POWER;
break;
case ACCEL_ID_KXTF9:
list[Accelerometer].maxRange = ACCEL_KXTF9_RANGE;
list[Accelerometer].resolution = ACCEL_KXTF9_RESOLUTION;
list[Accelerometer].power = ACCEL_KXTF9_POWER;
break;
case ACCEL_ID_BMA150:
list[Accelerometer].maxRange = ACCEL_BMA150_RANGE;
list[Accelerometer].resolution = ACCEL_BMA150_RESOLUTION;
list[Accelerometer].power = ACCEL_BMA150_POWER;
break;
case ACCEL_ID_BMA222:
list[Accelerometer].maxRange = ACCEL_BMA222_RANGE;
list[Accelerometer].resolution = ACCEL_BMA222_RESOLUTION;
list[Accelerometer].power = ACCEL_BMA222_POWER;
break;
case ACCEL_ID_BMA250:
list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
list[Accelerometer].power = ACCEL_BMA250_POWER;
break;
case ACCEL_ID_ADXL34X:
list[Accelerometer].maxRange = ACCEL_ADXL34X_RANGE;
list[Accelerometer].resolution = ACCEL_ADXL34X_RESOLUTION;
list[Accelerometer].power = ACCEL_ADXL34X_POWER;
break;
case ACCEL_ID_MMA8450:
list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE;
break;
case ACCEL_ID_MMA845X:
list[Accelerometer].maxRange = ACCEL_MMA845X_RANGE;
list[Accelerometer].resolution = ACCEL_MMA845X_RESOLUTION;
list[Accelerometer].power = ACCEL_MMA845X_POWER;
break;
case ACCEL_ID_MPU6050:
list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
list[Accelerometer].power = ACCEL_MPU6050_POWER;
break;
default:
LOGE("unknown accel id -- accel params will be wrong.");
break;
}
}
void MPLSensor::fillCompass(unsigned char compass, struct sensor_t *list)
{
switch (compass) {
case COMPASS_ID_AK8975:
list[MagneticField].maxRange = COMPASS_AKM8975_RANGE;
list[MagneticField].resolution = COMPASS_AKM8975_RESOLUTION;
list[MagneticField].power = COMPASS_AKM8975_POWER;
break;
case COMPASS_ID_AMI30X:
list[MagneticField].maxRange = COMPASS_AMI30X_RANGE;
list[MagneticField].resolution = COMPASS_AMI30X_RESOLUTION;
list[MagneticField].power = COMPASS_AMI30X_POWER;
break;
case COMPASS_ID_AMI306:
list[MagneticField].maxRange = COMPASS_AMI306_RANGE;
list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION;
list[MagneticField].power = COMPASS_AMI306_POWER;
break;
case COMPASS_ID_YAS529:
list[MagneticField].maxRange = COMPASS_YAS529_RANGE;
list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION;
list[MagneticField].power = COMPASS_AMI306_POWER;
break;
case COMPASS_ID_YAS530:
list[MagneticField].maxRange = COMPASS_YAS530_RANGE;
list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION;
list[MagneticField].power = COMPASS_YAS530_POWER;
break;
case COMPASS_ID_HMC5883:
list[MagneticField].maxRange = COMPASS_HMC5883_RANGE;
list[MagneticField].resolution = COMPASS_HMC5883_RESOLUTION;
list[MagneticField].power = COMPASS_HMC5883_POWER;
break;
case COMPASS_ID_MMC314X:
list[MagneticField].maxRange = COMPASS_MMC314X_RANGE;
list[MagneticField].resolution = COMPASS_MMC314X_RESOLUTION;
list[MagneticField].power = COMPASS_MMC314X_POWER;
break;
case COMPASS_ID_HSCDTD002B:
list[MagneticField].maxRange = COMPASS_HSCDTD002B_RANGE;
list[MagneticField].resolution = COMPASS_HSCDTD002B_RESOLUTION;
list[MagneticField].power = COMPASS_HSCDTD002B_POWER;
break;
case COMPASS_ID_HSCDTD004A:
list[MagneticField].maxRange = COMPASS_HSCDTD004A_RANGE;
list[MagneticField].resolution = COMPASS_HSCDTD004A_RESOLUTION;
list[MagneticField].power = COMPASS_HSCDTD004A_POWER;
break;
default:
LOGE("unknown compass id -- compass parameters will be wrong");
}
}
void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
{
if ((gyro != NULL) && (strcmp(gyro, "mpu3050") == 0)) {
list[Gyro].maxRange = GYRO_MPU3050_RANGE;
list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
list[Gyro].power = GYRO_MPU3050_POWER;
} else {
list[Gyro].maxRange = GYRO_MPU6050_RANGE;
list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
list[Gyro].power = GYRO_MPU6050_POWER;
}
return;
}
/* 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;
}