Filter: allow 1P LowPassFilter to work without alpha recalc per sample

this makes the 1P filter optionally syntax compatible with the 2P
filter and much more CPU efficient.
This commit is contained in:
Andrew Tridgell 2016-11-10 11:42:58 +11:00
parent 2a3d722d8c
commit f94e4b4375
2 changed files with 62 additions and 8 deletions

View File

@ -24,13 +24,29 @@ T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) {
_output = sample; _output = sample;
return _output; return _output;
} }
float rc = 1.0f/(M_2PI*cutoff_freq); 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; _output += (sample - _output) * alpha;
return _output; 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) // get latest filtered value from filter (equal to the value returned by latest call to apply method)
template <class T> template <class T>
const T &DigitalLPF<T>::get() const { const T &DigitalLPF<T>::get() const {
@ -61,6 +77,12 @@ void LowPassFilter<T>::set_cutoff_frequency(float cutoff_freq) {
_cutoff_freq = 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 // return the cutoff frequency
template <class T> template <class T>
float LowPassFilter<T>::get_cutoff_freq(void) const { 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); return _filter.apply(sample, _cutoff_freq, dt);
} }
template <class T>
T LowPassFilter<T>::apply(T sample) {
return _filter.apply(sample);
}
template <class T> template <class T>
const T &LowPassFilter<T>::get() const { const T &LowPassFilter<T>::get() const {
return _filter.get(); return _filter.get();

View File

@ -18,6 +18,30 @@
/// @brief A class to implement a low pass filter without losing precision even for int types /// @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 /// 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 /// 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 #pragma once
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
@ -27,21 +51,20 @@
template <class T> template <class T>
class DigitalLPF { class DigitalLPF {
public: public:
struct lpf_params {
float cutoff_freq;
float sample_freq;
float alpha;
};
DigitalLPF(); DigitalLPF();
// add a new raw value to the filter, retrieve the filtered result // 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, 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) // get latest filtered value from filter (equal to the value returned by latest call to apply method)
const T &get() const; const T &get() const;
void reset(T value); void reset(T value);
private: private:
T _output; T _output;
float alpha = 1.0f;
}; };
// LPF base class // LPF base class
@ -53,11 +76,15 @@ public:
// change parameters // change parameters
void set_cutoff_frequency(float cutoff_freq); void set_cutoff_frequency(float cutoff_freq);
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
// return the cutoff frequency // return the cutoff frequency
float get_cutoff_freq(void) const; float get_cutoff_freq(void) const;
T apply(T sample, float dt); T apply(T sample, float dt);
T apply(T sample);
const T &get() const; const T &get() const;
void reset(T value); void reset(T value);
void reset(void) { reset(T()); }
protected: protected:
float _cutoff_freq; float _cutoff_freq;