AP_InertialSensor: ensure gyro samples are used when the rate loop bu…

…ffer isn't
ArduPilot · Sep 26, 2024 · cc0198f · cc0198f
1 parent ceb6ac6
commit cc0198f
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 21 deletions.
diff --git 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
@@ -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
@@ -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()