From efec7723ff073197f1952c5bfb48745f3853fa49 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Thu, 2 Jul 2015 21:23:46 -0300 Subject: [PATCH] AP_InertialSensor: reset the MPU9250 chip on startup Now that the initialization of MPU9250 is shared between the AP_InertialSensor and other drivers using it as a backend, we can reset the MPU9250 in order to put it in a known state. --- .../AP_InertialSensor_MPU9250.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index ff114b538d..5ab4260e2d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -237,10 +237,22 @@ bool AP_InertialSensor_MPU9250::initialize_driver_state() { // initially run the bus at low speed spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - // Chip reset uint8_t tries; for (tries = 0; tries < 5; tries++) { + uint8_t user_ctrl = _register_read(spi, MPUREG_USER_CTRL); + + /* First disable the master I2C to avoid hanging the slaves on the + * aulixiliar I2C bus */ + if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) { + _register_write(spi, MPUREG_USER_CTRL, user_ctrl & ~BIT_USER_CTRL_I2C_MST_EN); + hal.scheduler->delay(10); + } + + // reset device + _register_write(spi, MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET); + hal.scheduler->delay(100); + // disable I2C as recommended by the datasheet _register_write(spi, MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);