From 3832bc455df83120258b3ebdeef1c4f6281c825f Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Thu, 16 Jan 2020 17:32:07 +0000
Subject: [PATCH] Plane: dynamic notch setting via FFT

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

diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index 184a121c2a..1b68588d25 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -491,6 +491,12 @@ void Plane::update_dynamic_notch()
         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
+#if HAL_GYROFFT_ENABLED
+        case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking
+            // set the harmonic notch filter frequency scaled on measured frequency
+            ins.update_harmonic_notch_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz());
+            break;
 #endif
         case HarmonicNotchDynamicMode::Fixed: // static
         default: