From bfd8d151c80d359ff61314d706d65c2bc588458e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 2 May 2017 10:59:52 +1000 Subject: [PATCH] AP_InertialSensor: lower slave rate with fast sampling On an invensense IMU with fast sampling we need to lower the slave sample rate for slave sensors such as the built-in AK8963 compass on a MPU9250. The slave rate is set as a multiple of the main rate, so it needs to be much lower for fast sampling. If we leave it high then it greatly impacts on IMU sample rate. Without this change a MPU9250 with fast sampling and a compass enabled will give a gyro rate of around 7200Hz. With this change it gets 7760Hz --- .../AP_InertialSensor/AP_InertialSensor_Invensense.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index b3d8352d48..1ff8ba0f90 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -814,6 +814,15 @@ void AP_InertialSensor_Invensense::_set_filter_register(void) // for logging purposes set the oversamping rate _set_accel_oversampling(_accel_instance, MPU_FIFO_DOWNSAMPLE_COUNT/2); _set_gyro_oversampling(_gyro_instance, MPU_FIFO_DOWNSAMPLE_COUNT); + + /* set divider for internal sample rate to 0x1F when fast + sampling enabled. This reduces the impact of the slave + sensor on the sample rate. It ends up with around 75Hz + slave rate, and reduces the impact on the gyro and accel + sample rate, ending up with around 7760Hz gyro rate and + 3880Hz accel rate + */ + _register_write(MPUREG_I2C_SLV4_CTRL, 0x1F); } }