Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Iampete1 2024-05-28 23:05:15 +01:00 committed by Andrew Tridgell
parent c908636cde
commit 51c77fe098
2 changed files with 12 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/Filter/tests/plot_harmonictest5.gnu Normal file → Executable file
View File

@ -8,5 +8,6 @@ set key autotitle columnhead
set xlabel "Attenuation(dB)" set xlabel "Attenuation(dB)"
set ylabel "PhaseLag(deg)" set ylabel "PhaseLag(deg)"
set key left bottom 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 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

16
libraries/Filter/tests/test_notchfilter.cpp
View File

@ -186,8 +186,8 @@ static void test_one_filter(float base_freq, float attenuation_dB,
double last_in; double last_in;
double last_out; double last_out;
double v_max; double v_max;
uint32_t last_crossing; double last_crossing;
uint32_t total_lag_samples; double total_lag_samples;
uint32_t lag_count; uint32_t lag_count;
float get_lag_degrees(const float freq) const { float get_lag_degrees(const float freq) const {
const float lag_avg = total_lag_samples/float(lag_count); 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); integral.v_max = MAX(integral.v_max, v);
} }
if (sample >= 0 && integral.last_in < 0) { 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) { if (v >= 0 && integral.last_out < 0 && integral.last_crossing > 0) {
integral.total_lag_samples += s - integral.last_crossing; const double crossing = (double)s + (v / (integral.last_out - v));
integral.total_lag_samples += crossing - integral.last_crossing;
integral.lag_count++; integral.lag_count++;
} }
integral.last_in = sample; integral.last_in = sample;