From b81179186320796808187303a30a9d9ef9e62c91 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 6 Aug 2024 16:01:26 +0100 Subject: [PATCH] Filter: examples: TransferFunctionCheck: add support for varable DT low pass --- .../TransferFunctionCheck/FilterTest.m | 15 ++++-- .../TransferFunctionCheck.cpp | 47 +++++++++++++++++-- 2 files changed, 54 insertions(+), 8 deletions(-) diff --git a/libraries/Filter/examples/TransferFunctionCheck/FilterTest.m b/libraries/Filter/examples/TransferFunctionCheck/FilterTest.m index bef9f6e0cb80e..73520f35b93e1 100644 --- a/libraries/Filter/examples/TransferFunctionCheck/FilterTest.m +++ b/libraries/Filter/examples/TransferFunctionCheck/FilterTest.m @@ -5,7 +5,7 @@ % File generated by the TransferFunctionCheck example filename = "test.csv"; -% There are some anoying warnings for text parsing and matrix solve +% There are some annoying warnings for text parsing and matrix solve warning('off','MATLAB:rankDeficientMatrix'); warning('off','MATLAB:table:ModifiedAndSavedVarnames'); @@ -21,6 +21,10 @@ % Try and work out types filters = {}; +filter_index = find(startsWith(lines,"LowPassFilterConstDtFloat")); +for i = 1:numel(filter_index) + filters{end+1} = parse_low_pass(lines, filter_index(i)); %#ok +end filter_index = find(startsWith(lines,"LowPassFilterFloat")); for i = 1:numel(filter_index) filters{end+1} = parse_low_pass(lines, filter_index(i)); %#ok @@ -102,7 +106,7 @@ end -% Caculalte using transfer function +% Calculate using transfer function Z = exp(1i*pi*(freq/(sample_freq*0.5))); for i = numel(filters):-1:1 @@ -113,7 +117,7 @@ % multiply all transfer functions together H_all = prod(H,2); -% Extract anmpitude and phase from transfer function +% Extract amplitude and phase from transfer function calc_amplitude = abs(H_all); calc_phase = atan2d(imag(H_all),real(H_all)); @@ -142,8 +146,9 @@ ylabel('phase') function ret = parse_low_pass(lines, start_index) - if ~startsWith(lines(start_index),"LowPassFilterFloat") - error("Expecting LowPassFilterFloat") + found = startsWith(lines(start_index),"LowPassFilterFloat") || startsWith(lines(start_index),"LowPassFilterConstDtFloat"); + if ~found + error("Expecting LowPassFilterConstDtFloat or LowPassFilterFloat") end % Next line gives sample rate and cutoff diff --git a/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp b/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp index db0cfcab7ed29..16d310914bbcb 100644 --- a/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp +++ b/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp @@ -16,7 +16,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); // Some helper classes to allow accessing protected variables, also useful for adding type specific prints -class LowPassHelper : public LowPassFilterConstDtFloat { +class LowPassConstDtHelper : public LowPassFilterConstDtFloat { public: using LowPassFilterConstDtFloat::LowPassFilterConstDtFloat; @@ -39,6 +39,33 @@ class LowPassHelper : public LowPassFilterConstDtFloat { float _sample_freq; }; +class LowPassHelper : public LowPassFilterFloat { +public: + using LowPassFilterFloat::LowPassFilterFloat; + + void set_cutoff_frequency_override(float sample_freq, float new_cutoff_freq) { + // Stash the DT so we can use it later + _DT = 1.0 / sample_freq; + set_cutoff_frequency(new_cutoff_freq); + } + + // Were really cheating here and using the same method as the filter to get the coefficient + // rather than pulling the coefficient directly + void print_transfer_function() { + hal.console->printf("LowPassFilterFloat\n"); + hal.console->printf("Sample rate: %.9f Hz, Cutoff: %.9f Hz\n", 1.0 / _DT, get_cutoff_freq()); + hal.console->printf("Low pass filter in the form: H(z) = a/(1-(1-a)*z^-1)\n"); + hal.console->printf("a: %.9f\n", calc_lowpass_alpha_dt(_DT, get_cutoff_freq())); + } + + float apply_override(const float sample) { + return apply(sample, _DT); + } + +private: + float _DT; +}; + class LowPass2pHelper : public LowPassFilter2pFloat { public: using LowPassFilter2pFloat::LowPassFilter2pFloat; @@ -67,12 +94,14 @@ class NotchHelper : public NotchFilterFloat { // create an instance each filter to test +LowPassConstDtHelper lowpassConstDt; LowPassHelper lowpass; LowPass2pHelper biquad; NotchHelper notch; NotchHelper notch2; enum class filter_type { + LowPassConstDT, LowPass, Biquad, Notch, @@ -97,7 +126,7 @@ void setup() const float sample_rate = 1000; const float target_freq = 50; - type = filter_type::Combination; + type = filter_type::LowPassConstDT; // Run 1000 time steps at each frequency const uint16_t num_samples = 1000; @@ -108,6 +137,11 @@ void setup() // Print transfer function of filter under test hal.console->printf("\n"); switch (type) { + case filter_type::LowPassConstDT: + lowpassConstDt.set_cutoff_frequency_override(sample_rate, target_freq); + lowpassConstDt.print_transfer_function(); + break; + case filter_type::LowPass: lowpass.set_cutoff_frequency_override(sample_rate, target_freq); lowpass.print_transfer_function(); @@ -147,6 +181,7 @@ void setup() void reset_all() { + lowpassConstDt.reset(0.0); lowpass.reset(0.0); biquad.reset(0.0); notch.reset(); @@ -156,8 +191,11 @@ void reset_all() float apply_to_filter_under_test(float input) { switch (type) { + case filter_type::LowPassConstDT: + return lowpassConstDt.apply(input); + case filter_type::LowPass: - return lowpass.apply(input); + return lowpass.apply_override(input); case filter_type::Biquad: return biquad.apply(input); @@ -195,6 +233,9 @@ void sweep(uint16_t num_samples, uint16_t max_freq, float sample_rate) hal.console->printf(", %+.9f", output); } hal.console->printf("\n"); + + // Try not to overflow the print buffer + hal.scheduler->delay(100); } }