From d872ca27cea97aa94545f0e4183f83271bf2773e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 17 Nov 2019 16:06:34 +1100
Subject: [PATCH] Plane: added dynamic harmonic notch support

---
 ArduPlane/ArduPlane.cpp |  3 ++-
 ArduPlane/Plane.h       |  1 +
 ArduPlane/system.cpp    | 42 +++++++++++++++++++++++++++++++++++++++++
 3 files changed, 45 insertions(+), 1 deletion(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index a3ae62028d..d767417007 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -107,8 +107,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(landing_gear_update, 5, 50),
 #endif
 #if EFI_ENABLED
-    SCHED_TASK(efi_update,             10,    200)
+    SCHED_TASK(efi_update,             10,    200),
 #endif
+    SCHED_TASK(update_dynamic_notch,   50,    200),
 };
 
 constexpr int8_t Plane::_failsafe_priorities[7];
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 0d51dcd681..1d9be93463 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -916,6 +916,7 @@ private:
     void startup_INS_ground(void);
     bool should_log(uint32_t mask);
     int8_t throttle_percentage(void);
+    void update_dynamic_notch();
     bool auto_takeoff_check(void);
     void takeoff_calc_roll(void);
     void takeoff_calc_pitch(void);
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index 1b1bc73948..33cd96a277 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -475,3 +475,45 @@ int8_t Plane::throttle_percentage(void)
     }
     return constrain_int16(throttle, -100, 100);
 }
+
+// update the harmonic notch filter center frequency dynamically
+void Plane::update_dynamic_notch()
+{
+    if (!ins.gyro_harmonic_notch_enabled()) {
+        return;
+    }
+    const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz();
+    const float ref = ins.get_gyro_harmonic_notch_reference();
+
+    if (is_zero(ref)) {
+        ins.update_harmonic_notch_freq_hz(ref_freq);
+        return;
+    }
+
+    switch (ins.get_gyro_harmonic_notch_tracking_mode()) {
+    case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking
+            // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
+            if (quadplane.available()) {
+                ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref)));
+            }
+            break;
+
+        case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking
+            if (rpm_sensor.healthy(0)) {
+                // set the harmonic notch filter frequency from the main rotor rpm
+                ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm_sensor.get_rpm(0) * ref / 60.0f));
+            } else {
+                ins.update_harmonic_notch_freq_hz(ref_freq);
+            }
+            break;
+#ifdef HAVE_AP_BLHELI_SUPPORT
+        case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
+            ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP_BLHeli::get_singleton()->get_average_motor_frequency_hz() * ref));
+            break;
+#endif
+        case HarmonicNotchDynamicMode::Fixed: // static
+        default:
+            ins.update_harmonic_notch_freq_hz(ref_freq);
+            break;
+    }
+}