uint8_t mpu_set_sensors(uint8_t sensors){
uint8_t data;
#ifdef USE_AK8963
uint8_t user_ctrl;
#endif
if (sensors & (MPU_XYZ_GYRO))
data = INV_CLK_PLL;
else if (sensors)
data = 0;
else
data = BIT_SLEEP;
if (mpu9250_writes(MPU6500_PWR_MGNT_1, 1, &data)) {
mpu9250_st.sensors = 0;
return -1;
}
mpu9250_st.clk_src = data & ~BIT_SLEEP;
data = 0;
if (!(sensors & MPU_X_GYRO))
data |= BIT_STBY_XG;
if (!(sensors & MPU_Y_GYRO))
data |= BIT_STBY_YG;
if (!(sensors & MPU_Z_GYRO))
data |= BIT_STBY_ZG;
if (!(sensors & MPU_XYZ_ACCEL))
data |= BIT_STBY_XYZA;
if (mpu9250_writes(MPU6500_PWR_MGNT_2, 1, &data)) {
mpu9250_st.sensors = 0;
return -1;
}
if (sensors && (sensors != MPU_XYZ_ACCEL))
/* Latched interrupts only used in LP accel mode. */
mpu_set_int_latched(0);
#ifdef USE_AK8963
if (mpu9250_reads(MPU6500_USER_CTRL, 1, &user_ctrl))
return -1;
/* Handle AKM power management. */
if (sensors & MPU_XYZ_COMPASS) {
data = AKM_SINGLE_MEASUREMENT;
user_ctrl |= BIT_AUX_IF_EN;
} else {
data = AKM_POWER_DOWN;
user_ctrl &= ~BIT_AUX_IF_EN;
}
if (mpu9250_st.dmp_on)
user_ctrl |= BIT_DMP_EN;
else
user_ctrl &= ~BIT_DMP_EN;
if (mpu9250_writes(AK8963_S1_DO, 1, &data))
return -1;
/* Enable/disable I2C master mode. */
if (mpu9250_writes(MPU6500_USER_CTRL, 1, &user_ctrl))
return -1;
#endif
mpu9250_st.sensors = sensors;
mpu9250_st.lp_accel_mode = 0;
delay_ms(50);
return 0;
}
inv_error_t MPU9250_DMP::setIntLatched(unsigned char enable)
{
return mpu_set_int_latched(enable);
}
