Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
inv_error_t MPU9250_DMP::setIntLatched(unsigned char enable)
{
	return mpu_set_int_latched(enable);
}