Plane: dynamic notch setting via FFT

5 years ago · 3832bc455d
1 changed files with 6 additions and 0 deletions
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@ -491,6 +491,12 @@ void Plane::update_dynamic_notch()
				@@ -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: