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); }