From f4521772c15d8d05fe383a284ead3d0946709277 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 6 Feb 2018 18:10:29 +1100 Subject: [PATCH] AP_InertialSensor: don't try fast sampling on a MPU6500 it can't do it --- libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index de2a4998bc..c14bb0b695 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -822,7 +822,7 @@ void AP_InertialSensor_Invensense::_set_filter_register(void) _backend_rate_hz = 1000; if (enable_fast_sampling(_accel_instance)) { - _fast_sampling = (_mpu_type != Invensense_MPU6000 && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI); + _fast_sampling = (_mpu_type >= Invensense_MPU9250 && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI); if (_fast_sampling) { if (get_sample_rate_hz() <= 1000) { _fifo_downsample_rate = 8;