diff --git a/libraries/Filter/LowPassFilter.cpp b/libraries/Filter/LowPassFilter.cpp index eba28c39228d34a60e12c9bddceefd2081a53278..be14b7148b0997f7378cd7caa3de3c8532726571 100644 --- a/libraries/Filter/LowPassFilter.cpp +++ b/libraries/Filter/LowPassFilter.cpp @@ -24,13 +24,29 @@ T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) { _output = sample; return _output; } - float rc = 1.0f/(M_2PI*cutoff_freq); - float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f); + alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f); _output += (sample - _output) * alpha; return _output; } +template <class T> +T DigitalLPF<T>::apply(const T &sample) { + _output += (sample - _output) * alpha; + return _output; +} + +template <class T> +void DigitalLPF<T>::compute_alpha(float sample_freq, float cutoff_freq) { + if (cutoff_freq <= 0.0f || sample_freq <= 0.0f) { + alpha = 1.0; + } else { + float dt = 1.0/sample_freq; + float rc = 1.0f/(M_2PI*cutoff_freq); + alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f); + } +} + // get latest filtered value from filter (equal to the value returned by latest call to apply method) template <class T> const T &DigitalLPF<T>::get() const { @@ -61,6 +77,12 @@ void LowPassFilter<T>::set_cutoff_frequency(float cutoff_freq) { _cutoff_freq = cutoff_freq; } +template <class T> +void LowPassFilter<T>::set_cutoff_frequency(float sample_freq, float cutoff_freq) { + _cutoff_freq = cutoff_freq; + _filter.compute_alpha(sample_freq, cutoff_freq); +} + // return the cutoff frequency template <class T> float LowPassFilter<T>::get_cutoff_freq(void) const { @@ -72,6 +94,11 @@ T LowPassFilter<T>::apply(T sample, float dt) { return _filter.apply(sample, _cutoff_freq, dt); } +template <class T> +T LowPassFilter<T>::apply(T sample) { + return _filter.apply(sample); +} + template <class T> const T &LowPassFilter<T>::get() const { return _filter.get(); diff --git a/libraries/Filter/LowPassFilter.h b/libraries/Filter/LowPassFilter.h index dffd006380da7548e2b08460e2c35d41ff3b3a71..6781e28b6757d597f7886aa25f3ddc9d76f23225 100644 --- a/libraries/Filter/LowPassFilter.h +++ b/libraries/Filter/LowPassFilter.h @@ -18,6 +18,30 @@ /// @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 + +/* + Note that this filter can be used in 2 ways: + + 1) providing dt on every sample, and calling apply like this: + + // call once + filter.set_cutoff_frequency(frequency_hz); + + // then on each sample + output = filter.apply(sample, dt); + + 2) providing a sample freq and cutoff_freq once at start + + // call once + filter.set_cutoff_frequency(sample_freq, frequency_hz); + + // then on each sample + output = filter.apply(sample); + + The second approach is more CPU efficient as it doesn't have to + recalculate alpha each time, but it assumes that dt is constant + */ + #pragma once #include <AP_Math/AP_Math.h> @@ -27,21 +51,20 @@ template <class T> class DigitalLPF { public: - struct lpf_params { - float cutoff_freq; - float sample_freq; - float alpha; - }; - 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); + + 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); private: T _output; + float alpha = 1.0f; }; // LPF base class @@ -53,11 +76,15 @@ public: // change parameters void set_cutoff_frequency(float cutoff_freq); + void set_cutoff_frequency(float sample_freq, 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) { reset(T()); } protected: float _cutoff_freq;