From 587471ab54958cbf0d7c19808a98fa7c47e780d5 Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Wed, 3 Jun 2015 17:46:45 -0300 Subject: [PATCH] AP_InertialSensor: disable I2C on MPU9250 As the datasheet says: "To prevent switching into I2C mode when using SPI, the I2C interface should be disabled by setting the I2C_IF_DIS configuration bit." We also reset the sensor like PX4Firmware does for initializing the MPU6000. See: https://github.com/diydrones/PX4Firmware/blob/ee1d8cd77069b3ba320d68f6dcb7222933c92057/src/drivers/mpu6000/mpu6000.cpp#L695 --- libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 3aa984cff8..48dcd8bebd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -421,8 +421,14 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void) // Chip reset uint8_t tries; for (tries = 0; tries<5; tries++) { + // reset device + _register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET); + hal.scheduler->delay(100); + // disable I2C as recommended by the datasheet + _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); + // Wake up device and select GyroZ clock. Note that the // MPU6000 starts up in sleep mode, and it can take some time // for it to come out of sleep