Filter: Testes: notch: interpolate crossing points for acurate phase lag

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
 

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;