- ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
- (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
- while (ao_mpu6000_reg_read(MPU6000_PWR_MGMT_1) &
- (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET))
- ao_yield();
-
- /* Reset signal conditioning */
- ao_mpu6000_reg_write(MPU6000_USER_CONTROL,
- (0 << MPU6000_USER_CONTROL_FIFO_EN) |
- (0 << MPU6000_USER_CONTROL_I2C_MST_EN) |
- (0 << MPU6000_USER_CONTROL_I2C_IF_DIS) |
- (0 << MPU6000_USER_CONTROL_FIFO_RESET) |
- (0 << MPU6000_USER_CONTROL_I2C_MST_RESET) |
- (1 << MPU6000_USER_CONTROL_SIG_COND_RESET));
-
- while (ao_mpu6000_reg_read(MPU6000_USER_CONTROL) & (1 << MPU6000_USER_CONTROL_SIG_COND_RESET))
- ao_yield();
+ _ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
+ (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
+
+ /* Wait for it to reset. If we talk too quickly, it appears to get confused */
+
+ _ao_mpu6000_wait_alive();
+
+ /* Reset signal conditioning, disabling I2C on SPI systems */
+ _ao_mpu6000_reg_write(MPU6000_USER_CTRL,
+ (0 << MPU6000_USER_CTRL_FIFO_EN) |
+ (0 << MPU6000_USER_CTRL_I2C_MST_EN) |
+ (AO_MPU6000_SPI << MPU6000_USER_CTRL_I2C_IF_DIS) |
+ (0 << MPU6000_USER_CTRL_FIFO_RESET) |
+ (0 << MPU6000_USER_CTRL_I2C_MST_RESET) |
+ (1 << MPU6000_USER_CTRL_SIG_COND_RESET));
+
+ while (_ao_mpu6000_reg_read(MPU6000_USER_CTRL) & (1 << MPU6000_USER_CTRL_SIG_COND_RESET))
+ ao_delay(AO_MS_TO_TICKS(10));