AP_InertialSensor: only publish used gyro values in main loop

andyp1per · Aug 23, 2024 · fdf257d · fdf257d
1 parent a5fcc33
commit fdf257d
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 34 deletions.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -821,7 +821,7 @@ 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
-    void push_next_gyro_sample(uint8_t instance);
+    bool push_next_gyro_sample(uint8_t instance, const Vector3f& gyro);
     // run the filter parmeter update code.
     void update_backend_filters();
 private:

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -169,16 +169,10 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &
         return;
     }
 
-#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
-    if (!_imu.push_rate_loop_gyro(instance)) { // rate loop thread is not consuming samples
-#endif
-        _imu._gyro[instance] = gyro;
+    _imu._gyro[instance] = gyro;
 #if HAL_GYROFFT_ENABLED
-        // copy the gyro samples from the backend to the frontend window for FFTs sampling at less than IMU rate
-        _imu._gyro_for_fft[instance] = _imu._last_gyro_for_fft[instance];
-#endif
-#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
-    }
+    // copy the gyro samples from the backend to the frontend window for FFTs sampling at less than IMU rate
+    _imu._gyro_for_fft[instance] = _imu._last_gyro_for_fft[instance];
 #endif
 
     _imu._gyro_healthy[instance] = true;
@@ -266,12 +260,16 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
             notch.filter[instance].reset();
         }
 #endif
-    } else {
-        _imu._gyro_filtered[instance] = gyro_filtered;
+        gyro_filtered = _imu._gyro_filtered[instance];
     }
 
 #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
-    _imu.push_next_gyro_sample(instance);
+    if (_imu.push_next_gyro_sample(instance, 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;
 #endif
 }
 

diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp
@@ -85,36 +85,26 @@ bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro)
 
     WITH_SEMAPHORE(_mutex);
 
-    if (_rate_loop_gyro_window.available() == 0) {
-        return false;  // we thought we had a sample, but concurrency means we actually do not
-    }
-
     return _rate_loop_gyro_window.pop(gyro);
 }
 
-void AP_InertialSensor::push_next_gyro_sample(uint8_t instance)
+bool AP_InertialSensor::push_next_gyro_sample(uint8_t instance, const Vector3f& gyro)
 {
-    if (push_rate_loop_gyro(instance)) {
+    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
         */
-        if (++fast_rate_buffer->rate_decimation_count >= fast_rate_buffer->rate_decimation) {
-
-            WITH_SEMAPHORE(fast_rate_buffer->_mutex);
-
-            if (!fast_rate_buffer->_rate_loop_gyro_window.push(_gyro_filtered[instance])) {
-                debug("dropped rate loop sample");
-            }
-            fast_rate_buffer->rate_decimation_count = 0;
-            // semaphore is already held so we can directly publish the gyro data
-            _gyro[instance] = _gyro_filtered[instance];
-#if HAL_GYROFFT_ENABLED
-            // copy the gyro samples from the backend to the frontend window for FFTs sampling at less than IMU rate
-            _gyro_for_fft[instance] = _last_gyro_for_fft[instance];
-#endif
-            fast_rate_buffer->_notifier.signal();
+        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;
     }
+    return false;
 }
 
 void AP_InertialSensor::update_backend_filters()