Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -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

9
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);
alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
_output += (sample - _output) * alpha;
if (!initialised) {
initialised = true;
_output = sample;
}
return _output;
}
template <class T>
T DigitalLPF<T>::apply(const T &sample) {
_output += (sample - _output) * alpha;
if (!initialised) {
initialised = true;
_output = sample;
}
return _output;
}
@ -54,6 +62,7 @@ const T &DigitalLPF<T>::get() const {
template <class T>
void DigitalLPF<T>::reset(T value) {
_output = value;
initialised = true;
}
////////////////////////////////////////////////////////////////////////////////////////////

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