diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 287287ff7c..0581ef34cb 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -41,6 +41,10 @@ # define I2C_MST_CLOCK_400KHZ 0x0D # define I2C_MST_CLOCK_258KHZ 0x08 +#define MPUREG_I2C_SLV4_CTRL 0x34 +#define MPUREG_I2C_MST_DELAY_CTRL 0x67 +# define I2C_SLV0_DLY_EN 0x01 + #define AK8963_I2C_ADDR 0x0c #define AK8963_WIA 0x00 @@ -451,6 +455,11 @@ bool AP_AK8963_SerialBus_MPU9250::start_measurements() { const uint8_t count = sizeof(struct raw_value); + /* Don't sample AK8963 at MPU9250's sample rate. See MPU9250's datasheet + * about registers below and registers 73-96, External Sensor Data */ + _write(MPUREG_I2C_SLV4_CTRL, 31); + _write(MPUREG_I2C_MST_DELAY_CTRL, I2C_SLV0_DLY_EN); + /* Configure the registers from AK8963 that will be read by MPU9250's * master: we will get the result directly from MPU9250's registers starting * from MPUREG_EXT_SENS_DATA_00 when read_raw() is called */