Copter: setup the right PID notch sample rate

when using the rate thread the PID notches run at a very different sample rate
ArduPilot · Feb 12, 2024 · 6a37231 · 6a37231
1 parent 3dc5f8e
commit 6a37231
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 3 deletions.
diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
@@ -14,12 +14,21 @@ void Copter::rate_controller_thread()
 
     uint32_t last_run_us = AP_HAL::micros();
     float dt_avg = 0.0;
-    uint32_t last_report_ms = AP_HAL::millis();
+    uint32_t now_ms = AP_HAL::millis();
+    uint32_t last_report_ms = now_ms;
+    uint32_t last_notch_sample_ms = now_ms;
+    bool was_using_rate_thread = false;
 
     while (true) {
         // allow changing option at runtime
         if (!flight_option_is_set(FlightOptions::USE_RATE_LOOP_THREAD)) {
             using_rate_thread = false;
+            if (was_using_rate_thread) {
+                // if we were using the rate thread, we need to
+                // setup the notch filter sample rate
+                attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
+                was_using_rate_thread = false;
+            }
             hal.scheduler->delay_microseconds(500);
             last_run_us = AP_HAL::micros();
             continue;
@@ -56,11 +65,21 @@ void Copter::rate_controller_thread()
         hal.scheduler->delay_microseconds(100);
 #endif
 
-        const uint32_t now_ms = AP_HAL::millis();
+        now_ms = AP_HAL::millis();
+
+        if (now_ms - last_notch_sample_ms >= 1000 || !was_using_rate_thread) {
+            // update the PID notch sample rate at 1Hz if if we are
+            // enabled at runtime
+            last_notch_sample_ms = now_ms;
+            attitude_control->set_notch_sample_rate(1.0 / dt_avg);
+        }
+
         if (now_ms - last_report_ms >= 2000) {
             last_report_ms = now_ms;
             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Rate: %.2fHz", 1.0/dt_avg);
         }
+
+        was_using_rate_thread = true;
     }
 }
 

diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
@@ -674,7 +674,9 @@ void Copter::one_hz_loop()
     AP_Notify::flags.flying = !ap.land_complete;
 
     // slowly update the PID notches with the average loop rate
-    attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
+    if (!using_rate_thread) {
+        attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
+    }
     pos_control->get_accel_z_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
 #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
     custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());