From cc0198f6aea0f7624212aa6870771368085d98a7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 27 Aug 2024 22:36:22 +0100 Subject: [PATCH] AP_InertialSensor: ensure gyro samples are used when the rate loop buffer isn't --- .../AP_InertialSensor/AP_InertialSensor.h | 6 ++-- .../AP_InertialSensor_Backend.cpp | 8 +++-- .../AP_InertialSensor/FastRateBuffer.cpp | 31 +++++++++---------- 3 files changed, 24 insertions(+), 21 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 0c5f47706374c8..b9963a121d077a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -821,11 +821,11 @@ class AP_InertialSensor : AP_AccelCal_Client // are gyro samples being sourced from the rate loop buffer bool use_rate_loop_gyro_samples() const; // push a new gyro sample into the fast rate buffer - bool push_next_gyro_sample(uint8_t instance, const Vector3f& gyro); + bool push_next_gyro_sample(const Vector3f& gyro); // run the filter parmeter update code. void update_backend_filters(); -private: - bool push_rate_loop_gyro(uint8_t instance) const; + // are rate loop samples enabled for this instance? + bool is_rate_loop_gyro_enabled(uint8_t instance) const; }; namespace AP { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 0dffa4370bf5a3..301d4071261b02 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -264,8 +264,12 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const } #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED - if (_imu.push_next_gyro_sample(instance, gyro_filtered)) { - // if we used the value, record it for publication to the front-end + if (_imu.is_rate_loop_gyro_enabled(instance)) { + if (_imu.push_next_gyro_sample(gyro_filtered)) { + // if we used the value, record it for publication to the front-end + _imu._gyro_filtered[instance] = gyro_filtered; + } + } else { _imu._gyro_filtered[instance] = gyro_filtered; } #else diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp index 8c072498c0e21f..6617915fecd62f 100644 --- a/libraries/AP_InertialSensor/FastRateBuffer.cpp +++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp @@ -59,7 +59,7 @@ bool AP_InertialSensor::use_rate_loop_gyro_samples() const } // whether or not to push the current gyro sample -bool AP_InertialSensor::push_rate_loop_gyro(uint8_t instance) const +bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const { return use_rate_loop_gyro_samples() && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); } @@ -88,23 +88,22 @@ bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro) return _rate_loop_gyro_window.pop(gyro); } -bool AP_InertialSensor::push_next_gyro_sample(uint8_t instance, const Vector3f& gyro) +bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro) { - if (push_rate_loop_gyro(instance) - && ++fast_rate_buffer->rate_decimation_count >= fast_rate_buffer->rate_decimation) { - /* - tell the rate thread we have a new sample - */ - WITH_SEMAPHORE(fast_rate_buffer->_mutex); - - if (!fast_rate_buffer->_rate_loop_gyro_window.push(gyro)) { - debug("dropped rate loop sample"); - } - fast_rate_buffer->rate_decimation_count = 0; - fast_rate_buffer->_notifier.signal(); - return true; + if (++fast_rate_buffer->rate_decimation_count < fast_rate_buffer->rate_decimation) { + return false; + } + /* + tell the rate thread we have a new sample + */ + WITH_SEMAPHORE(fast_rate_buffer->_mutex); + + if (!fast_rate_buffer->_rate_loop_gyro_window.push(gyro)) { + debug("dropped rate loop sample"); } - return false; + fast_rate_buffer->rate_decimation_count = 0; + fast_rate_buffer->_notifier.signal(); + return true; } void AP_InertialSensor::update_backend_filters()