From 51c77fe098dc546654784c075010a8a74533e830 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Tue, 28 May 2024 23:05:15 +0100
Subject: [PATCH] Filter: Testes: notch: interpolate crossing points for
 acurate phase lag

---
 libraries/Filter/tests/plot_harmonictest5.gnu |  1 +
 libraries/Filter/tests/test_notchfilter.cpp   | 16 +++++++++++-----
 2 files changed, 12 insertions(+), 5 deletions(-)
 mode change 100644 => 100755 libraries/Filter/tests/plot_harmonictest5.gnu

diff --git a/libraries/Filter/tests/plot_harmonictest5.gnu b/libraries/Filter/tests/plot_harmonictest5.gnu
old mode 100644
new mode 100755
index 92283141aafb2..4de8b27b730a3
--- a/libraries/Filter/tests/plot_harmonictest5.gnu
+++ b/libraries/Filter/tests/plot_harmonictest5.gnu
@@ -8,5 +8,6 @@ set key autotitle columnhead
 set xlabel "Attenuation(dB)"
 set ylabel "PhaseLag(deg)"
 set key left bottom
+set yrange [0:60]
 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 6b60611c28af1..ce6d6ffa3a4b6 100644
--- a/libraries/Filter/tests/test_notchfilter.cpp
+++ b/libraries/Filter/tests/test_notchfilter.cpp
@@ -186,8 +186,8 @@ static void test_one_filter(float base_freq, float attenuation_dB,
         double last_in;
         double last_out;
         double v_max;
-        uint32_t last_crossing;
-        uint32_t total_lag_samples;
+        double last_crossing;
+        double 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);
@@ -218,10 +218,16 @@ static void test_one_filter(float base_freq, float attenuation_dB,
             integral.v_max = MAX(integral.v_max, v);
         }
         if (sample >= 0 && integral.last_in < 0) {
-            integral.last_crossing = s;
+            // Always interpolating the value at 0.0
+            // crossing happened some fraction before the current sample
+            // result in the range -1.0 to 0.0
+            // linear interpolation: ((0.0 - last_in) / (sample - last_in)) - 1.0 is the same as:
+            // sample / (last_in - sample)
+            integral.last_crossing = (double)s + (sample / (integral.last_in - sample));
         }
-        if (v >= 0 && integral.last_out < 0 && integral.last_crossing != 0) {
-            integral.total_lag_samples += s - integral.last_crossing;
+        if (v >= 0 && integral.last_out < 0 && integral.last_crossing > 0) {
+            const double crossing = (double)s + (v / (integral.last_out - v));
+            integral.total_lag_samples += crossing - integral.last_crossing;
             integral.lag_count++;
         }
         integral.last_in = sample;