/* Handle sensor on/off combinations. */
static void setup_gyro(void)
{
unsigned char mask = 0, lp_accel_was_on = 0;
if (hal.sensors & ACCEL_ON)
mask |= INV_XYZ_ACCEL;
if (hal.sensors & GYRO_ON) {
mask |= INV_XYZ_GYRO;
lp_accel_was_on |= hal.lp_accel_mode;
}
#ifdef COMPASS_ENABLED
if (hal.sensors & COMPASS_ON) {
mask |= INV_XYZ_COMPASS;
lp_accel_was_on |= hal.lp_accel_mode;
}
#endif
/* If you need a power transition, this function should be called with a
* mask of the sensors still enabled. The driver turns off any sensors
* excluded from this mask.
*/
mpu_set_sensors(mask);
mpu_configure_fifo(mask);
if (lp_accel_was_on) {
unsigned short rate;
hal.lp_accel_mode = 0;
/* Switching out of LP accel, notify MPL of new accel sampling rate. */
mpu_get_sample_rate(&rate);
inv_set_accel_sample_rate(1000000L / rate);
}
}
static void tap_cb(unsigned char direction, unsigned char count)
{
switch (direction) {
case TAP_X_UP:
MPL_LOGI("Tap X+ ");
break;
case TAP_X_DOWN:
MPL_LOGI("Tap X- ");
break;
case TAP_Y_UP:
MPL_LOGI("Tap Y+ ");
break;
case TAP_Y_DOWN:
MPL_LOGI("Tap Y- ");
break;
case TAP_Z_UP:
MPL_LOGI("Tap Z+ ");
break;
case TAP_Z_DOWN:
MPL_LOGI("Tap Z- ");
break;
default:
return;
}
MPL_LOGI("x%d\n", count);
return;
}
static void android_orient_cb(unsigned char orientation)
{
switch (orientation) {
case ANDROID_ORIENT_PORTRAIT:
MPL_LOGI("Portrait\n");
break;
case ANDROID_ORIENT_LANDSCAPE:
MPL_LOGI("Landscape\n");
break;
case ANDROID_ORIENT_REVERSE_PORTRAIT:
MPL_LOGI("Reverse Portrait\n");
break;
case ANDROID_ORIENT_REVERSE_LANDSCAPE:
MPL_LOGI("Reverse Landscape\n");
break;
default:
return;
}
}
static inline void run_self_test(void)
{
int result;
long gyro[3], accel[3];
#if defined (MPU6500) || defined (MPU9250)
result = mpu_run_6500_self_test(gyro, accel, 0);
#elif defined (MPU6050) || defined (MPU9150)
result = mpu_run_self_test(gyro, accel);
#endif
if (result == 0x7) {
MPL_LOGI("Passed!\n");
MPL_LOGI("accel: %7.4f %7.4f %7.4f\n",
accel[0]/65536.f,
accel[1]/65536.f,
accel[2]/65536.f);
MPL_LOGI("gyro: %7.4f %7.4f %7.4f\n",
gyro[0]/65536.f,
gyro[1]/65536.f,
gyro[2]/65536.f);
/* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/
#ifdef USE_CAL_HW_REGISTERS
/*
* This portion of the code uses the HW offset registers that are in the MPUxxxx devices
* instead of pushing the cal data to the MPL software library
*/
unsigned char i = 0;
#if defined (MPU6500) || defined (MPU9250)
mpu_set_accel_bias_6500_reg(accel);
#elif defined (MPU6050) || defined (MPU9150)
mpu_set_accel_bias_6050_reg(accel);
#endif
#else
/* Push the calibrated data to the MPL library.
*
* MPL expects biases in hardware units << 16, but self test returns
* biases in g's << 16.
*/
unsigned short accel_sens;
float gyro_sens;
