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

Filter: fixed low pass 2p filter to auto-init

Browse Source 
this prevents the init to zero for temperature filters
This commit is contained in:
Andrew Tridgell 2021-01-11 08:09:28 +11:00 committed by Peter Barker
parent 4495414d05
commit cf47c19c0d
2 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/Filter/LowPassFilter2p.cpp
View File

@ -17,6 +17,11 @@ T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &par
return sample;
}
if (!initialised) {
reset(sample);
initialised = true;
}
T delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2;
T output = delay_element_0 * params.b0 + _delay_element_1 * params.b1 + _delay_element_2 * params.b2;
@ -29,11 +34,13 @@ T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &par
template <class T>
void DigitalBiquadFilter<T>::reset() {
_delay_element_1 = _delay_element_2 = T();
initialised = false;
}
template <class T>
void DigitalBiquadFilter<T>::reset(const T &value) {
_delay_element_1 = _delay_element_2 = value;
initialised = true;
}
template <class T>

1
libraries/Filter/LowPassFilter2p.h
View File

@ -45,6 +45,7 @@ public:
private:
T _delay_element_1;
T _delay_element_2;
bool initialised;
};
template <class T>