From 9bc9873e38551c02bc812fc48fd05276ddeeccb3 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Mon, 8 Mar 2021 21:05:24 +0000
Subject: [PATCH] AP_Vehicle: optionally run the harmonic notch update at the
 loop rate

---
 libraries/AP_Vehicle/AP_Vehicle.cpp | 14 +++++++++++++-
 libraries/AP_Vehicle/AP_Vehicle.h   |  4 ++++
 2 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
index e814e0dcbf..7c3d68d9d6 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.cpp
+++ b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -219,7 +219,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_GyroFFT,   &vehicle.gyro_fft,       update,                  400, 50),
     SCHED_TASK_CLASS(AP_GyroFFT,   &vehicle.gyro_fft,       update_parameters,         1, 50),
 #endif
-    SCHED_TASK(update_dynamic_notch,                   200,    200),
+    SCHED_TASK(update_dynamic_notch,             LOOP_RATE,    200),
     SCHED_TASK_CLASS(AP_VideoTX,   &vehicle.vtx,            update,                    2, 100),
     SCHED_TASK(send_watchdog_reset_statustext,         0.1,     20),
 #if HAL_WITH_ESC_TELEM
@@ -332,6 +332,18 @@ void AP_Vehicle::write_notch_log_messages() const
             notches[0], notches[1], notches[2], notches[3]);
 }
 
+// run notch update at either loop rate or 200Hz
+void AP_Vehicle::update_dynamic_notch_at_specified_rate()
+{
+    const uint32_t now = AP_HAL::millis();
+
+    if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::LoopRateUpdate)
+        || now - _last_notch_update_ms > 5) {
+        update_dynamic_notch();
+        _last_notch_update_ms = now;
+    }
+}
+
 // reboot the vehicle in an orderly manner, doing various cleanups and
 // flashing LEDs as appropriate
 void AP_Vehicle::reboot(bool hold_in_bootloader)
diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
index 880ea4e586..f001a5cfff 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.h
+++ b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -345,8 +345,12 @@ private:
     // statustext:
     void send_watchdog_reset_statustext();
 
+    // run notch update at either loop rate or 200Hz
+    void update_dynamic_notch_at_specified_rate();
+
     bool likely_flying;         // true if vehicle is probably flying
     uint32_t _last_flying_ms;   // time when likely_flying last went true
+    uint32_t _last_notch_update_ms; // last time update_dynamic_notch() was run
 
     static AP_Vehicle *_singleton;