Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Filter: added auto-init to LowPassFilter

Browse Source 
make sure initial filter returns are not a long way off if filtered
input is far from zero
This commit is contained in:
Andrew Tridgell 2021-04-04 09:14:57 +10:00
parent be161744db
commit 6cae97446a
2 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/Filter/LowPassFilter.cpp
View File

@ -27,12 +27,20 @@ T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) {
float rc = 1.0f/(M_2PI*cutoff_freq); float rc = 1.0f/(M_2PI*cutoff_freq);
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;
if (!initialised) {
initialised = true;
_output = sample;
}
return _output; return _output;
} }
template <class T> template <class T>
T DigitalLPF<T>::apply(const T &sample) { T DigitalLPF<T>::apply(const T &sample) {
_output += (sample - _output) * alpha; _output += (sample - _output) * alpha;
if (!initialised) {
initialised = true;
_output = sample;
}
return _output; return _output;
} }
@ -53,7 +61,8 @@ const T &DigitalLPF<T>::get() const {
template <class T> template <class T>
void DigitalLPF<T>::reset(T value) { void DigitalLPF<T>::reset(T value) {
_output = value; _output = value;
initialised = true;
} }
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////

8
libraries/Filter/LowPassFilter.h
View File

@ -61,10 +61,14 @@ public:
// 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);
void reset() {
initialised = false;
}
private: private:
T _output; T _output;
float alpha = 1.0f; float alpha = 1.0f;
bool initialised;
}; };
// LPF base class // LPF base class
@ -85,8 +89,8 @@ public:
T apply(T sample); 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()); } void reset(void) { _filter.reset(); }
protected: protected:
float _cutoff_freq; float _cutoff_freq;