From 0d10f74bf71146d863b0e08573b691cd51ab28b4 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Sun, 27 Aug 2023 20:01:05 +0100
Subject: [PATCH] Plane: update PID notch centers at 1Hz with average loop rate

---
 ArduPlane/ArduPlane.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 61d7642533..36bc4f5b53 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -357,6 +357,16 @@ void Plane::one_second_loop()
         !is_equal(G_Dt, scheduler.get_loop_period_s())) {
         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     }
+
+    const float loop_rate = AP::scheduler().get_filtered_loop_rate_hz();
+#if HAL_QUADPLANE_ENABLED
+    if (quadplane.available()) {
+        quadplane.attitude_control->set_notch_sample_rate(loop_rate);
+    }
+#endif
+    rollController.set_notch_sample_rate(loop_rate);
+    pitchController.set_notch_sample_rate(loop_rate);
+    yawController.set_notch_sample_rate(loop_rate);
 }
 
 void Plane::three_hz_loop()