Filter: added test for attenuation adjustment

libraries/Filter/tests/plot_harmonictest2.gnu

@@ -0,0 +1,11 @@
+#!/usr/bin/gnuplot -persist
+set y2tics 0,10
+set ytics nomirror
+set style data linespoints
+set key autotitle
+set datafile separator ","
+set key autotitle columnhead
+set xlabel "Freq(Hz)"
+set ylabel "Attenuation(dB)"
+#set ylabel2 "PhaseLag(deg)"
+plot "harmonicnotch_test2.csv" using 1:2 axis x1y1, "harmonicnotch_test2.csv" using 1:3 axis x1y2

libraries/Filter/tests/plot_harmonictest3.gnu

@@ -0,0 +1,12 @@
+#!/usr/bin/gnuplot -persist
+set y2tics 0,10
+set ytics nomirror
+set style data linespoints
+set key autotitle
+set datafile separator ","
+set key autotitle columnhead
+set xlabel "Freq(Hz)"
+set ylabel "Attenuation(dB)"
+#set ylabel2 "PhaseLag(deg)"
+plot "harmonicnotch_test3.csv" using 1:2 axis x1y1, "harmonicnotch_test3.csv" using 1:3 axis x1y2, "harmonicnotch_test3.csv" using 1:4 axis x1y1, "harmonicnotch_test3.csv" using 1:5 axis x1y2
+

libraries/Filter/tests/plot_harmonictest4.gnu

@@ -0,0 +1,12 @@
+#!/usr/bin/gnuplot -persist
+set y2tics 0,10
+set ytics nomirror
+set style data linespoints
+set key autotitle
+set datafile separator ","
+set key autotitle columnhead
+set xlabel "Freq(Hz)"
+set ylabel "Attenuation(dB)"
+set key left bottom
+plot "harmonicnotch_test4.csv" using 1:2, "harmonicnotch_test4.csv" using 1:3, "harmonicnotch_test4.csv" using 1:4, "harmonicnotch_test4.csv" using 1:5, "harmonicnotch_test4.csv" using 1:6, "harmonicnotch_test4.csv" using 1:7, "harmonicnotch_test4.csv" using 1:8
+

libraries/Filter/tests/test_notchfilter.cpp

@@ -6,6 +6,12 @@
 
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
+static double ratio_to_dB(double ratio)
+{
+    return 10*log(ratio);
+}
+
+#if 0
 /*
   test if a reset of a notch filter results in no glitch in the following samples
   with constant input
@@ -162,5 +168,164 @@ TEST(NotchFilterTest, HarmonicNotchTest)
     EXPECT_NEAR(integrals[4].get_lag_degrees(5), 55.23, 0.5);
     EXPECT_NEAR(integrals[9].get_lag_degrees(10), 112.23, 0.5);
 }
+#endif
+
+
+/*
+  calculate attenuation and phase lag for a single harmonic notch filter
+ */
+static void test_one_filter(float base_freq, float attenuation_dB,
+                            float bandwidth, float test_freq, float source_freq,
+                            uint16_t harmonics, uint16_t options,
+                            float &phase_lag, float &out_attenuation_dB)
+{
+    const uint16_t rate_hz = 2000;
+    const uint32_t samples = 50000;
+    const float test_amplitude = 1.0;
+    const double dt = 1.0 / rate_hz;
+
+    HarmonicNotchFilter<float> filter {};
+    struct {
+        double last_in;
+        double last_out;
+        double v_max;
+        uint32_t last_crossing;
+        uint32_t total_lag_samples;
+        uint32_t lag_count;
+        float get_lag_degrees(const float freq) const {
+            const float lag_avg = total_lag_samples/float(lag_count);
+            return (360.0 * lag_avg * freq) / rate_hz;
+        }
+    } integral {};
+
+    auto &f = filter;
+    const uint8_t num_harmonics = __builtin_popcount(harmonics);
+
+    HarmonicNotchFilterParams notch_params {};
+    notch_params.set_options(options);
+    f.allocate_filters(num_harmonics, harmonics, notch_params.num_composite_notches());
+    notch_params.set_attenuation(attenuation_dB);
+    notch_params.set_bandwidth_hz(bandwidth);
+    notch_params.set_center_freq_hz(base_freq);
+    notch_params.set_freq_min_ratio(1.0);
+    f.init(rate_hz, notch_params);
+    f.update(source_freq);
+
+    for (uint32_t s=0; s<samples; s++) {
+        const double t = s * dt;
+
+        const double sample = sin(test_freq * t * 2 * M_PI) * test_amplitude;
+        float v = sample;
+        v = f.apply(v);
+        if (s >= samples/10) {
+            integral.v_max = MAX(integral.v_max, v);
+        }
+        if (sample >= 0 && integral.last_in < 0) {
+            integral.last_crossing = s;
+        }
+        if (v >= 0 && integral.last_out < 0 && integral.last_crossing != 0) {
+            integral.total_lag_samples += s - integral.last_crossing;
+            integral.lag_count++;
+        }
+        integral.last_in = sample;
+        integral.last_out = v;
+        f.update(source_freq);
+    }
+    phase_lag = integral.get_lag_degrees(test_freq);
+    out_attenuation_dB = ratio_to_dB(integral.v_max/test_amplitude);
+}
+
+/*
+  test the test_one_filter function
+ */
+TEST(NotchFilterTest, HarmonicNotchTest2)
+{
+    const float min_freq = 1.0;
+    const float max_freq = 200;
+    const uint16_t steps = 1000;
+
+    const char *csv_file = "harmonicnotch_test2.csv";
+    FILE *f = fopen(csv_file, "w");
+    fprintf(f, "Freq(Hz),Attenuation(dB),Lag(deg)\n");
+
+    for (uint16_t i=0; i<steps; i++) {
+        float attenuation_dB, phase_lag;
+        const float test_freq = min_freq + i*(max_freq-min_freq)/steps;
+        test_one_filter(46, 60, 23, test_freq, 46, 1, 0, phase_lag, attenuation_dB);
+        fprintf(f, "%.1f,%f,%f\n", test_freq, attenuation_dB, phase_lag);
+    }
+    fclose(f);
+}
+
+/*
+  test behaviour with TreatLowAsMin, we expect attenuation to decrease
+  as we get close to zero source frequency and phase lag to approach
+  zero
+ */
+TEST(NotchFilterTest, HarmonicNotchTest3)
+{
+    const float min_freq = 1.0;
+    const float max_freq = 200;
+    const uint16_t steps = 1000;
+
+    const char *csv_file = "harmonicnotch_test3.csv";
+    FILE *f = fopen(csv_file, "w");
+    fprintf(f, "Freq(Hz),Attenuation1(dB),Lag1(deg),Attenuation2(dB),Lag2(deg)\n");
+
+    const float source_freq = 35;
+    for (uint16_t i=0; i<steps; i++) {
+        float attenuation_dB1, phase_lag1;
+        float attenuation_dB2, phase_lag2;
+        const float test_freq = min_freq + i*(max_freq-min_freq)/steps;
+        // first with TreatLowAsMin
+        test_one_filter(50, 30, 25, test_freq, source_freq, 1, uint16_t(HarmonicNotchFilterParams::Options::TreatLowAsMin),
+                        phase_lag1,
+                        attenuation_dB1);
+        // and without
+        test_one_filter(50, 30, 25, test_freq, source_freq, 1, 0,
+                        phase_lag2,
+                        attenuation_dB2);
+        fprintf(f, "%.1f,%f,%f,%f,%f\n", test_freq, attenuation_dB1, phase_lag1, attenuation_dB2, phase_lag2);
+
+        // the phase lag with the attenuation adjustment should not be
+        // more than without for frequencies below min freq
+        if (test_freq < 50) {
+            EXPECT_LE(phase_lag2, phase_lag1);
+        }
+    }
+    fclose(f);
+}
+
+/*
+  show the progress of a multi-harmonic notch as the source frequency drops below the min frequency
+ */
+TEST(NotchFilterTest, HarmonicNotchTest4)
+{
+    const float min_freq = 1.0;
+    const float max_freq = 250;
+    const uint16_t steps = 1000;
+
+    const char *csv_file = "harmonicnotch_test4.csv";
+    FILE *f = fopen(csv_file, "w");
+    fprintf(f, "Freq(Hz),60Hz(dB),50Hz(dB),40Hz(dB),30Hz(dB),20Hz(dB),10Hz(dB),0Hz(dB)\n");
+
+    for (uint16_t i=0; i<steps; i++) {
+        float phase_lag;
+        const float source_freq[7] { 60, 50, 40, 30, 20, 10, 0 };
+        float attenuations[7];
+        const float test_freq = min_freq + i*(max_freq-min_freq)/steps;
+        for (uint8_t F = 0; F < ARRAY_SIZE(source_freq); F++) {
+            test_one_filter(50, 30, 25, test_freq, source_freq[F], 15, 0,
+                            phase_lag,
+                            attenuations[F]);
+        }
+        fprintf(f, "%.1f,%f,%f,%f,%f,%f,%f,%f\n",
+                test_freq,
+                attenuations[0], attenuations[1], attenuations[2],
+                attenuations[3], attenuations[4], attenuations[5],
+                attenuations[6]);
+    }
+    fclose(f);
+}
 
 AP_GTEST_MAIN()