diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 8c6f05fd1e6789..0b781ffe8f2084 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -577,10 +577,10 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { #endif // @Param: _GYRO_RATE - // @DisplayName: Gyro rate for IMUs with Fast Sampling enabled - // @Description: Gyro rate for IMUs with fast sampling enabled. The gyro rate is the sample rate at which the IMU filters operate and needs to be at least double the maximum filter frequency. If the sensor does not support the selected rate the next highest supported rate will be used. For IMUs which do not support fast sampling this setting is ignored and the default gyro rate of 1Khz is used. + // @DisplayName: Gyro rate multiplier for IMUs with Fast Sampling enabled + // @Description: Gyro rate multipier for IMUs with fast sampling enabled. The gyro rate is the sample rate multiplier of base output data rate at which the IMU filters operate and needs to be at least double the maximum filter frequency. If the sensor does not support the selected rate the next highest supported rate will be used. For IMUs which do not support fast sampling this setting is ignored and the default gyro rate is used. // @User: Advanced - // @Values: 0:1kHz,1:2kHz,2:4kHz,3:8kHz + // @Values: 0:x1,1:x2,2:x4,3:x8 // @RebootRequired: True AP_GROUPINFO("_GYRO_RATE", 42, AP_InertialSensor, _fast_sampling_rate, MPU_FIFO_FASTSAMPLE_DEFAULT), diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 817cb2e947c858..1ca816dad41d81 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -319,7 +319,7 @@ class AP_InertialSensor_Backend return (HAL_INS_HIGHRES_SAMPLE & (1U< base_odr) { - loop_limit = 2; + uint8_t min_base_rate_multiplier = 1; + if (get_loop_rate_hz() > base_backend_rate) { + min_base_rate_multiplier = 2; } - if (get_loop_rate_hz() > base_odr * 2) { - loop_limit = 4; + if (get_loop_rate_hz() > base_backend_rate * 2) { + min_base_rate_multiplier = 4; } // constrain the gyro rate to be a 2^N multiple - uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8); + uint8_t fast_sampling_rate_multiplier = constrain_int16(get_fast_sampling_rate(), min_base_rate_multiplier, 8); // calculate rate we will be giving samples to the backend - return constrain_int16(base_odr * fast_sampling_rate, base_odr, max_odr); + return constrain_int16(base_backend_rate * fast_sampling_rate_multiplier, base_backend_rate, max_backend_rate); } /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index 6b4fc49a8c5c49..36d83c433d5f44 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -58,7 +58,7 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend void set_filter_and_scaling_icm42670(void); void set_filter_and_scaling_icm456xy(void); void fifo_reset(); - uint16_t calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const; + uint16_t calculate_fast_sampling_backend_rate(uint16_t base_backend_rate, uint16_t max_backend_rate) const; /* Read samples from FIFO */ void read_fifo();