From dc0ec25d4afff2aa9f91044dfa33fe80967d9ea4 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 27 Jul 2024 13:12:50 +0100 Subject: [PATCH 1/9] APM_Control: move to new constant dt low pass filter class --- libraries/APM_Control/AP_AutoTune.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h index 67b6cf07f9036..72dccd5dd95d6 100644 --- a/libraries/APM_Control/AP_AutoTune.h +++ b/libraries/APM_Control/AP_AutoTune.h @@ -128,9 +128,9 @@ class AP_AutoTune // 5 point mode filter for FF estimate ModeFilterFloat_Size5 ff_filter; - LowPassFilterFloat actuator_filter; - LowPassFilterFloat rate_filter; - LowPassFilterFloat target_filter; + LowPassFilterConstDtFloat actuator_filter; + LowPassFilterConstDtFloat rate_filter; + LowPassFilterConstDtFloat target_filter; // separate slew limiters for P and D float slew_limit_max, slew_limit_tau; From 1e51e12f9daa6c77999f4fb64eba80306e3051fd Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 27 Jul 2024 13:12:50 +0100 Subject: [PATCH 2/9] AP_GyroFFT: move to new constant dt low pass filter class --- libraries/AP_GyroFFT/AP_GyroFFT.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.h b/libraries/AP_GyroFFT/AP_GyroFFT.h index 19b501b719358..115448227b308 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.h +++ b/libraries/AP_GyroFFT/AP_GyroFFT.h @@ -157,7 +157,7 @@ class AP_GyroFFT } private: - LowPassFilterFloat _lowpass_filter[XYZ_AXIS_COUNT]; + LowPassFilterConstDtFloat _lowpass_filter[XYZ_AXIS_COUNT]; FilterWithBuffer _median_filter[XYZ_AXIS_COUNT]; }; @@ -311,7 +311,7 @@ class AP_GyroFFT // smoothing filter on the bandwidth MedianLowPassFilter3dFloat _center_bandwidth_filter[FrequencyPeak::MAX_TRACKED_PEAKS]; // smoothing filter on the frequency fit - LowPassFilterFloat _harmonic_fit_filter[XYZ_AXIS_COUNT]; + LowPassFilterConstDtFloat _harmonic_fit_filter[XYZ_AXIS_COUNT]; // configured sampling rate uint16_t _fft_sampling_rate_hz; From 313d87c9def1791c7e56b31e321de531cc56b77f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 27 Jul 2024 13:12:50 +0100 Subject: [PATCH 3/9] AP_ICEngine: move to new constant dt low pass filter class --- libraries/AP_ICEngine/AP_ICEngine.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h index 351302e7638f0..b9582a872ed53 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -83,7 +83,7 @@ class AP_ICEngine { #if AP_RPM_ENABLED // filter for RPM value - LowPassFilterFloat _rpm_filter; + LowPassFilterConstDtFloat _rpm_filter; float filtered_rpm_value; #endif From 087d0e42ff18ff10ff2b644fcc1b7510f3c75011 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 27 Jul 2024 13:12:50 +0100 Subject: [PATCH 4/9] AP_InertialSensor: move to new constant dt low pass filter class --- libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h | 2 +- libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index 522dfd527d3c3..7ba132f3a309a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -182,7 +182,7 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend Vector3f gyro; uint8_t accel_count; uint8_t gyro_count; - LowPassFilterVector3f accel_filter{4000, 188}; + LowPassFilterConstDtVector3f accel_filter{4000, 188}; } _accum; }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h index b6e21d0034d1d..1b14062863a29 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h @@ -152,7 +152,7 @@ class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend Vector3f gyro; uint8_t accel_count; uint8_t gyro_count; - LowPassFilterVector3f accel_filter{4500, 188}; + LowPassFilterConstDtVector3f accel_filter{4500, 188}; } _accum; }; From 5a70b8e61bf8e6b939a5582ab8d693577b97e916 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 27 Jul 2024 13:14:08 +0100 Subject: [PATCH 5/9] copter: flowhold: move to constant dt filter and provide dt in runtime cutoff frequency update --- ArduCopter/mode.h | 2 +- ArduCopter/mode_flowhold.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 3dde86df7dff0..20ecf6ba296da 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -984,7 +984,7 @@ class ModeFlowHold : public Mode { // calculate attitude from flow data void flow_to_angle(Vector2f &bf_angle); - LowPassFilterVector2f flow_filter; + LowPassFilterConstDtVector2f flow_filter; bool flowhold_init(bool ignore_checks); void flowhold_run(); diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 1e53107c6b622..947c6496c9986 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -240,7 +240,7 @@ void ModeFlowHold::run() // check for filter change if (!is_equal(flow_filter.get_cutoff_freq(), flow_filter_hz.get())) { - flow_filter.set_cutoff_frequency(flow_filter_hz.get()); + flow_filter.set_cutoff_frequency(copter.scheduler.get_loop_rate_hz(), flow_filter_hz.get()); } // get pilot desired climb rate From 9ea4cebeca38cd1d3d7faeb3c3a92823b6764338 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 27 Jul 2024 13:14:34 +0100 Subject: [PATCH 6/9] SITL: SIM_Battery: provde dt in voltage filter apply function --- libraries/SITL/SIM_Battery.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_Battery.cpp b/libraries/SITL/SIM_Battery.cpp index d5a7219c36d77..4c94bd305140b 100644 --- a/libraries/SITL/SIM_Battery.cpp +++ b/libraries/SITL/SIM_Battery.cpp @@ -149,7 +149,7 @@ void Battery::set_current(float current) voltage = get_resting_voltage(100 * remaining_Ah / capacity_Ah) - voltage_delta; } - voltage_filter.apply(voltage); + voltage_filter.apply(voltage, dt); { const uint64_t temperature_dt = now - temperature.last_update_micros; From a6bbeb2c3fcf200cecc1eff2b0b774784705ee93 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 27 Jul 2024 13:15:17 +0100 Subject: [PATCH 7/9] Filter: example: TransferFunctionCheck: override constant dt filter --- .../TransferFunctionCheck/TransferFunctionCheck.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp b/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp index 320f0b44562a4..db0cfcab7ed29 100644 --- a/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp +++ b/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp @@ -16,20 +16,20 @@ 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 LowPassFilterFloat { +class LowPassHelper : public LowPassFilterConstDtFloat { public: - using LowPassFilterFloat::LowPassFilterFloat; + using LowPassFilterConstDtFloat::LowPassFilterConstDtFloat; - void set_cutoff_frequency_override(float sample_freq, float cutoff_freq) { + void set_cutoff_frequency_override(float sample_freq, float new_cutoff_freq) { // Stash the sample rate so we can use it later _sample_freq = sample_freq; - set_cutoff_frequency(sample_freq, cutoff_freq); + set_cutoff_frequency(sample_freq, 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("LowPassFilterConstDtFloat\n"); hal.console->printf("Sample rate: %.9f Hz, Cutoff: %.9f Hz\n", _sample_freq, 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(1.0/_sample_freq, get_cutoff_freq())); From 262b34f2f96b7c60cd6624f627f6bb22b65fbfab Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 27 Jul 2024 13:16:20 +0100 Subject: [PATCH 8/9] Filter: LowPassFilter: split into two classes for constant and variable dt --- libraries/Filter/LowPassFilter.cpp | 148 ++++++++++++++--------------- libraries/Filter/LowPassFilter.h | 104 ++++++++++---------- 2 files changed, 129 insertions(+), 123 deletions(-) diff --git a/libraries/Filter/LowPassFilter.cpp b/libraries/Filter/LowPassFilter.cpp index e4f741b93431d..eba8e0d38bf06 100644 --- a/libraries/Filter/LowPassFilter.cpp +++ b/libraries/Filter/LowPassFilter.cpp @@ -1,8 +1,6 @@ // /// @file LowPassFilter.cpp -/// @brief A class to implement a low pass filter without losing precision even for int types -/// the downside being that it's a little slower as it internally uses a float -/// and it consumes an extra 4 bytes of memory to hold the constant gain +/// @brief A class to implement a low pass filter #ifndef HAL_DEBUG_BUILD #define AP_INLINE_VECTOR_OPS @@ -12,134 +10,134 @@ #include //////////////////////////////////////////////////////////////////////////////////////////// -// DigitalLPF +// DigitalLPF, base class //////////////////////////////////////////////////////////////////////////////////////////// template DigitalLPF::DigitalLPF() { // built in initialization - _output = T(); + output = T(); } // add a new raw value to the filter, retrieve the filtered result template -T DigitalLPF::apply(const T &sample, float cutoff_freq, float dt) { - if (is_negative(cutoff_freq) || is_negative(dt)) { - INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); - _output = sample; - return _output; - } - if (is_zero(cutoff_freq)) { - _output = sample; - return _output; - } - if (is_zero(dt)) { - return _output; - } - float rc = 1.0f/(M_2PI*cutoff_freq); - alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f); - _output += (sample - _output) * alpha; - if (!initialised) { - initialised = true; - _output = sample; - } - return _output; -} - -template -T DigitalLPF::apply(const T &sample) { - _output += (sample - _output) * alpha; +T DigitalLPF::_apply(const T &sample, const float &alpha) { + output += (sample - output) * alpha; if (!initialised) { initialised = true; - _output = sample; - } - return _output; -} - -template -void DigitalLPF::compute_alpha(float sample_freq, float cutoff_freq) { - if (sample_freq <= 0) { - alpha = 1; - } else { - alpha = calc_lowpass_alpha_dt(1.0/sample_freq, cutoff_freq); + output = sample; } + return output; } // get latest filtered value from filter (equal to the value returned by latest call to apply method) template const T &DigitalLPF::get() const { - return _output; + return output; } +// Reset filter to given value template -void DigitalLPF::reset(T value) { - _output = value; +void DigitalLPF::reset(const T &value) { + output = value; initialised = true; } - -//////////////////////////////////////////////////////////////////////////////////////////// -// LowPassFilter -//////////////////////////////////////////////////////////////////////////////////////////// -// constructors +// Set reset flag such that the filter will be reset to the next value applied template -LowPassFilter::LowPassFilter() : - _cutoff_freq(0.0f) {} +void DigitalLPF::reset() { + initialised = false; +} -template -LowPassFilter::LowPassFilter(float cutoff_freq) : - _cutoff_freq(cutoff_freq) {} +template class DigitalLPF; +template class DigitalLPF; +template class DigitalLPF; + +//////////////////////////////////////////////////////////////////////////////////////////// +// Low pass filter with constant time step +//////////////////////////////////////////////////////////////////////////////////////////// +// constructor template -LowPassFilter::LowPassFilter(float sample_freq, float cutoff_freq) +LowPassFilterConstDt::LowPassFilterConstDt(const float &sample_freq, const float &new_cutoff_freq) { - set_cutoff_frequency(sample_freq, cutoff_freq); + set_cutoff_frequency(sample_freq, new_cutoff_freq); } // change parameters template -void LowPassFilter::set_cutoff_frequency(float cutoff_freq) { - _cutoff_freq = cutoff_freq; +void LowPassFilterConstDt::set_cutoff_frequency(const float &sample_freq, const float &new_cutoff_freq) { + cutoff_freq = new_cutoff_freq; + + if (sample_freq <= 0) { + alpha = 1; + } else { + alpha = calc_lowpass_alpha_dt(1.0/sample_freq, cutoff_freq); + } } +// return the cutoff frequency template -void LowPassFilter::set_cutoff_frequency(float sample_freq, float cutoff_freq) { - _cutoff_freq = cutoff_freq; - _filter.compute_alpha(sample_freq, cutoff_freq); +float LowPassFilterConstDt::get_cutoff_freq(void) const { + return cutoff_freq; } -// return the cutoff frequency +// add a new raw value to the filter, retrieve the filtered result template -float LowPassFilter::get_cutoff_freq(void) const { - return _cutoff_freq; +T LowPassFilterConstDt::apply(const T &sample) { + return this->_apply(sample, alpha); } +template class LowPassFilterConstDt; +template class LowPassFilterConstDt; +template class LowPassFilterConstDt; + +//////////////////////////////////////////////////////////////////////////////////////////// +// Low pass filter with variable time step +//////////////////////////////////////////////////////////////////////////////////////////// + template -T LowPassFilter::apply(T sample, float dt) { - return _filter.apply(sample, _cutoff_freq, dt); +LowPassFilter::LowPassFilter(const float &new_cutoff_freq) +{ + set_cutoff_frequency(new_cutoff_freq); } +// change parameters template -T LowPassFilter::apply(T sample) { - return _filter.apply(sample); +void LowPassFilter::set_cutoff_frequency(const float &new_cutoff_freq) { + cutoff_freq = new_cutoff_freq; } +// return the cutoff frequency template -const T &LowPassFilter::get() const { - return _filter.get(); +float LowPassFilter::get_cutoff_freq() const { + return cutoff_freq; } +// add a new raw value to the filter, retrieve the filtered result template -void LowPassFilter::reset(T value) { - _filter.reset(value); +T LowPassFilter::apply(const T &sample, const float &dt) { + if (is_negative(cutoff_freq) || is_negative(dt)) { + INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); + this->reset(sample); + return this->get(); + } + if (is_zero(cutoff_freq)) { + this->reset(sample); + return this->get(); + } + if (is_zero(dt)) { + return this->get(); + } + const float rc = 1.0f/(M_2PI*cutoff_freq); + const float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f); + return this->_apply(sample, alpha); } /* * Make an instances * Otherwise we have to move the constructor implementations to the header file :P */ -template class LowPassFilter; -template class LowPassFilter; template class LowPassFilter; template class LowPassFilter; template class LowPassFilter; diff --git a/libraries/Filter/LowPassFilter.h b/libraries/Filter/LowPassFilter.h index 497d0ddf976e7..3106bc14aeabf 100644 --- a/libraries/Filter/LowPassFilter.h +++ b/libraries/Filter/LowPassFilter.h @@ -15,14 +15,12 @@ // /// @file LowPassFilter.h -/// @brief A class to implement a low pass filter without losing precision even for int types -/// the downside being that it's a little slower as it internally uses a float -/// and it consumes an extra 4 bytes of memory to hold the constant gain +/// @brief A class to implement a low pass filter. /* - Note that this filter can be used in 2 ways: + Two classes are provided: - 1) providing dt on every sample, and calling apply like this: + LowPassFilter: providing dt on every sample, and calling apply like this: // call once filter.set_cutoff_frequency(frequency_hz); @@ -30,7 +28,7 @@ // then on each sample output = filter.apply(sample, dt); - 2) providing a sample freq and cutoff_freq once at start + LowPassFilterConstDt: providing a sample freq and cutoff_freq once at start // call once filter.set_cutoff_frequency(sample_freq, frequency_hz); @@ -45,79 +43,89 @@ #pragma once #include -#include "FilterClass.h" // DigitalLPF implements the filter math template class DigitalLPF { public: + + // constructor DigitalLPF(); - // add a new raw value to the filter, retrieve the filtered result - T apply(const T &sample, float cutoff_freq, float dt); - T apply(const T &sample); CLASS_NO_COPY(DigitalLPF); - void compute_alpha(float sample_freq, float cutoff_freq); - // get latest filtered value from filter (equal to the value returned by latest call to apply method) const T &get() const; - void reset(T value); - void reset() { - initialised = false; - } + + // Reset filter to given value + void reset(const T &value); + + // Set reset flag such that the filter will be reset to the next value applied + void reset(); + +protected: + // add a new raw value to the filter, retrieve the filtered result + T _apply(const T &sample, const float &alpha); private: - T _output; - float alpha = 1.0f; + T output; bool initialised; }; -// LPF base class +// Low pass filter with constant time step template -class LowPassFilter { +class LowPassFilterConstDt : public DigitalLPF { public: - LowPassFilter(); - LowPassFilter(float cutoff_freq); - LowPassFilter(float sample_freq, float cutoff_freq); - CLASS_NO_COPY(LowPassFilter); + // constructors + LowPassFilterConstDt() {}; + LowPassFilterConstDt(const float &sample_freq, const float &cutoff_freq); + + CLASS_NO_COPY(LowPassFilterConstDt); // change parameters - void set_cutoff_frequency(float cutoff_freq); - void set_cutoff_frequency(float sample_freq, float cutoff_freq); + void set_cutoff_frequency(const float &sample_freq, const float &cutoff_freq); // return the cutoff frequency - float get_cutoff_freq(void) const; - T apply(T sample, float dt); - T apply(T sample); - const T &get() const; - void reset(T value); - void reset(void) { _filter.reset(); } + float get_cutoff_freq() const; -protected: - float _cutoff_freq; + // add a new raw value to the filter, retrieve the filtered result + T apply(const T &sample); private: - DigitalLPF _filter; + float cutoff_freq; + float alpha; }; -// Uncomment this, if you decide to remove the instantiations in the implementation file -/* -template -LowPassFilter::LowPassFilter() : _cutoff_freq(0.0f) { - -} -// constructor +typedef LowPassFilterConstDt LowPassFilterConstDtFloat; +typedef LowPassFilterConstDt LowPassFilterConstDtVector2f; +typedef LowPassFilterConstDt LowPassFilterConstDtVector3f; + +// Low pass filter with variable time step template -LowPassFilter::LowPassFilter(float cutoff_freq) : _cutoff_freq(cutoff_freq) { - -} -*/ +class LowPassFilter : public DigitalLPF { +public: + + // constructors + LowPassFilter() {}; + LowPassFilter(const float &cutoff_freq); + + CLASS_NO_COPY(LowPassFilter); + + // change parameters + void set_cutoff_frequency(const float &cutoff_freq); + + // return the cutoff frequency + float get_cutoff_freq() const; + + // add a new raw value to the filter, retrieve the filtered result + T apply(const T &sample, const float &dt); + +private: + float cutoff_freq; +}; // typedefs for compatibility -typedef LowPassFilter LowPassFilterInt; -typedef LowPassFilter LowPassFilterLong; typedef LowPassFilter LowPassFilterFloat; typedef LowPassFilter LowPassFilterVector2f; typedef LowPassFilter LowPassFilterVector3f; From d4a61db3ac137c3824eacc5330e47badebab2f8e Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 6 Aug 2024 16:01:26 +0100 Subject: [PATCH 9/9] 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); } }