From bf12c686c70e77b7a682b23f7a3a9e857605fc38 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Sat, 12 Oct 2019 13:04:45 +0100
Subject: [PATCH] ArduCopter: add support for BLHeli telemetry-based updates to
 the harmonic notch

refactor to include RPM for all copter types
---
 ArduCopter/defines.h  |  8 ++++++++
 ArduCopter/system.cpp | 34 ++++++++++++++++++++++++----------
 2 files changed, 32 insertions(+), 10 deletions(-)

diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 4eec6ec0cb..b36a7496ee 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -172,6 +172,14 @@ enum LoggingParameters {
      LOG_SYSIDS_MSG,
 };
 
+// Harmonic notch update mode
+enum HarmonicNotchDynamicMode {
+    HarmonicNotch_Fixed,
+    HarmonicNotch_UpdateThrottle,
+    HarmonicNotch_UpdateRPM,
+    HarmonicNotch_UpdateBLHeli,
+};
+
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)
 #define MASK_LOG_ATTITUDE_MED           (1<<1)
 #define MASK_LOG_GPS                    (1<<2)
diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp
index 653e8189bb..b85c62ed5c 100755
--- a/ArduCopter/system.cpp
+++ b/ArduCopter/system.cpp
@@ -1,4 +1,5 @@
 #include "Copter.h"
+#include <AP_BLHeli/AP_BLHeli.h>
 
 /*****************************************************************************
 *   The init_ardupilot function processes everything we need for an in - air restart
@@ -294,18 +295,31 @@ void Copter::update_dynamic_notch()
         return;
     }
 
-    if (is_tradheli()) {
+    switch (ins.get_gyro_harmonic_notch_tracking_mode()) {
+        case HarmonicNotch_UpdateThrottle: // throttle based tracking
+            // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
+            ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref)));
+            break;
+
 #if RPM_ENABLED == ENABLED
-        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);
-        }
+        case HarmonicNotch_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;
 #endif
-    } else {
-        // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
-        ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref)));
+#ifdef HAVE_AP_BLHELI_SUPPORT
+        case HarmonicNotch_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 HarmonicNotch_Fixed: // static
+        default:
+            ins.update_harmonic_notch_freq_hz(ref_freq);
+            break;
     }
 }