AP_GyroFFT: correct notch calculation for FFT tune.

use freq_min_ratio for scaling
add harmonic calculation test
set harmonics in notch setup

libraries/AP_GyroFFT/AP_GyroFFT.cpp

@@ -701,7 +701,40 @@ void AP_GyroFFT::start_notch_tune()
     if (!hal.dsp->fft_start_average(_state)) {
         gcs().send_text(MAV_SEVERITY_WARNING, "FFT: Unable to start FFT averaging");
     }
+    // throttle averaging for average fft calculation
     _avg_throttle_out = 0.0f;
+#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    AP_Motors* motors = AP::motors();
+    if (motors != nullptr) {
+        _avg_throttle_out = motors->get_throttle_hover();
+    }
+#endif
+}
+
+// calculate the frequency to be used for the harmonic notch
+float AP_GyroFFT::calculate_notch_frequency(float* freqs, uint16_t numpeaks, float harmonic_fit, uint8_t& harmonics)
+{
+    float harmonic = freqs[0];
+    harmonics = 1;
+
+    for (uint8_t i = 1; i < numpeaks; i++) {
+        if (freqs[i] < harmonic) {
+            for (uint8_t x = 2; x <=HNF_MAX_HARMONICS; x++) {
+                if (is_harmonic_of(harmonic, freqs[i], x, harmonic_fit)) {
+                    harmonic = freqs[i];
+                }
+            }
+        }
+    }
+    // select the harmonics that were matched
+    for (uint8_t i = 0; i < numpeaks; i++) {
+        for (uint8_t x = 1; x <=HNF_MAX_HARMONICS; x++) {
+            if (is_harmonic_of(freqs[i], harmonic, x, harmonic_fit)) {
+                harmonics |= 1<<(x - 1);
+            }
+        }
+    }
+    return harmonic;
 }
 
 void AP_GyroFFT::stop_notch_tune()
@@ -718,21 +751,11 @@ void AP_GyroFFT::stop_notch_tune()
         return;
     }
 
-    float harmonic = freqs[0];
+    uint8_t harmonics;
-    float harmonic_fit = 100.0f;
+    float harmonic = calculate_notch_frequency(freqs, numpeaks, _harmonic_fit, harmonics);
-
-    for (uint8_t i = 1; i < numpeaks; i++) {
-        if (freqs[i] < harmonic) {
-            const float fit = 100.0f * fabsf(harmonic - freqs[i] * _harmonic_multiplier) / harmonic;
-            if (isfinite(fit) && fit < _harmonic_fit && fit < harmonic_fit) {
-                harmonic = freqs[i];
-                harmonic_fit = fit;
-            }
-        }
-    }
 
     gcs().send_text(MAV_SEVERITY_INFO, "FFT: Found peaks at %.1f/%.1f/%.1fHz", freqs[0], freqs[1], freqs[2]);
-    gcs().send_text(MAV_SEVERITY_NOTICE, "FFT: Selected %.1fHz with fit %.1f%%\n", harmonic, is_equal(harmonic_fit, 100.0f) ? 100.0f : 100.0f - harmonic_fit);
+    gcs().send_text(MAV_SEVERITY_NOTICE, "FFT: Selected %.1fHz\n", harmonic);
 
     // if we don't have a throttle value then all bets are off
     if (is_zero(_avg_throttle_out) || is_zero(harmonic)) {
@@ -740,17 +763,14 @@ void AP_GyroFFT::stop_notch_tune()
         return;
     }
 
-    // pick a ref which means the notch covers all the way down to FFT_MINHZ
+    if (!_ins->setup_throttle_gyro_harmonic_notch(harmonic, (float)_fft_min_hz.get(), _avg_throttle_out, harmonics)) {
-    const float thr_ref = _avg_throttle_out * sq((float)_fft_min_hz.get() / harmonic);
-
-    if (!_ins->setup_throttle_gyro_harmonic_notch((float)_fft_min_hz.get(), thr_ref)) {
         gcs().send_text(MAV_SEVERITY_WARNING, "FFT: Unable to set throttle notch with %.1fHz/%.2f",
-            (float)_fft_min_hz.get(), thr_ref);
+            harmonic, _avg_throttle_out);
         // save results to FFT slots
-        _throttle_ref.set(thr_ref);
+        _throttle_ref.set(_avg_throttle_out);
         _freq_hover_hz.set(harmonic);
     } else {
-        gcs().send_text(MAV_SEVERITY_NOTICE, "FFT: Notch frequency %.1fHz and ref %.2f selected", (float)_fft_min_hz.get(), thr_ref);
+        gcs().send_text(MAV_SEVERITY_NOTICE, "FFT: Notch frequency %.1fHz and ref %.2f selected", harmonic, _avg_throttle_out);
     }
 }
 

libraries/AP_GyroFFT/AP_GyroFFT.h

@@ -113,6 +113,11 @@ public:
     bool check_esc_noise() const { return (_options & uint32_t(Options::ESCNoiseCheck)) != 0; }
     // look for a frequency in the detected noise
     float has_noise_at_frequency_hz(float freq) const;
+    static float calculate_notch_frequency(float* freqs, uint16_t numpeaks, float harmonic_fit, uint8_t& harmonics);
+    static bool is_harmonic_of(float harmonic, float fundamental, uint8_t mult, float _fit) {
+        const float fit = 100.0f * fabsf(harmonic - fundamental * mult) / harmonic;
+        return (isfinite(fit) && fit < _fit);
+    }
 
     static const struct AP_Param::GroupInfo var_info[];
     static AP_GyroFFT *get_singleton() { return _singleton; }

libraries/AP_GyroFFT/tests/test_harmonic_fit.cpp

@@ -0,0 +1,23 @@
+#include <AP_gtest.h>
+#include <AP_HAL/HAL.h>
+#include <AP_HAL/Util.h>
+#include <AP_GyroFFT/AP_GyroFFT.h>
+#include <stdio.h>
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+TEST(harmonic_fit_Test, Basic)
+{
+#if HAL_GYROFFT_ENABLED
+    float freqs[] { 176.9, 57.2, 228.7 };
+    uint8_t harmonics;
+    float harmonic = AP_GyroFFT::calculate_notch_frequency(freqs, 3, 10, harmonics);
+
+    printf("FFT: Found peaks at %.1f/%.1f/%.1fHz\n", freqs[0], freqs[1], freqs[2]);
+    printf("FFT: Selected %.1fHz %d\n", harmonic, harmonics);
+
+    EXPECT_TRUE(is_equal(harmonic, 57.2f));
+#endif
+}
+
+AP_GTEST_MAIN()

libraries/AP_GyroFFT/tests/wscript

@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    bld.ap_find_tests(
+        use='ap',
+    )