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::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::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::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;
//ALOGV_IF(EXTRA_VERBOSE, "accel data: %f %f %f", s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2]);
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::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
ALOGW("orienHandler: data not valid (%d)", (int) res);
}
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) {
ALOGW(
"compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d",
res);
}
s->magnetic.status = estimateCompassAccuracy();
if (res == INV_SUCCESS)
*pending_mask |= (1 << index);
}