From c908636cde8651d2006e30c3d22f43051184688c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 26 May 2024 19:27:16 +1000
Subject: [PATCH] Filter: added test of phase lag vs attenuation

---
 libraries/Filter/tests/plot_harmonictest5.gnu | 12 +++++++
 libraries/Filter/tests/test_notchfilter.cpp   | 32 +++++++++++++++++++
 2 files changed, 44 insertions(+)
 create mode 100644 libraries/Filter/tests/plot_harmonictest5.gnu

diff --git a/libraries/Filter/tests/plot_harmonictest5.gnu b/libraries/Filter/tests/plot_harmonictest5.gnu
new file mode 100644
index 0000000000000..92283141aafb2
--- /dev/null
+++ b/libraries/Filter/tests/plot_harmonictest5.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 "Attenuation(dB)"
+set ylabel "PhaseLag(deg)"
+set key left bottom
+plot "harmonicnotch_test5.csv" using 1:2, "harmonicnotch_test5.csv" using 1:3, "harmonicnotch_test5.csv" using 1:4, "harmonicnotch_test5.csv" using 1:5, "harmonicnotch_test5.csv" using 1:6
+
diff --git a/libraries/Filter/tests/test_notchfilter.cpp b/libraries/Filter/tests/test_notchfilter.cpp
index a13c9aeae161a..6b60611c28af1 100644
--- a/libraries/Filter/tests/test_notchfilter.cpp
+++ b/libraries/Filter/tests/test_notchfilter.cpp
@@ -325,4 +325,36 @@ TEST(NotchFilterTest, HarmonicNotchTest4)
     fclose(f);
 }
 
+/*
+  show phase lag versus attenuation
+ */
+TEST(NotchFilterTest, HarmonicNotchTest5)
+{
+    const float min_attenuation = 0;
+    const float max_attenuation = 50;
+    const uint16_t steps = 200;
+
+    const char *csv_file = "harmonicnotch_test5.csv";
+    FILE *f = fopen(csv_file, "w");
+    fprintf(f, "Attenuation(dB),Lag(10Hz),Lag(20Hz),Lag(30Hz),Lag(40Hz),Lag(50Hz),Lag(60Hz),Lag(70Hz)\n");
+
+    for (uint16_t i=0; i<steps; i++) {
+        const float test_freq[7] { 10, 20, 30, 40, 50, 60, 70 };
+        float phase_lags[7];
+        float attenuations[7];
+        const float test_attenuation = min_attenuation + i*(max_attenuation-min_attenuation)/steps;
+        for (uint8_t F = 0; F < ARRAY_SIZE(test_freq); F++) {
+            test_one_filter(60, test_attenuation, 60, test_freq[F], 50, 1, 0,
+                            phase_lags[F],
+                            attenuations[F]);
+        }
+        fprintf(f, "%.3f,%f,%f,%f,%f,%f,%f,%f\n",
+                test_attenuation,
+                phase_lags[0], phase_lags[1], phase_lags[2],
+                phase_lags[3], phase_lags[4], phase_lags[5],
+                phase_lags[6]);
+    }
+    fclose(f);
+}
+
 AP_GTEST_MAIN()