Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

Filter: fixed reset of notch filters

Browse Source 
when we reset a notch we need to init the stored values to the current
value, rather than assuming that zero is the right value

this matters when switching IMUs in flight when we are only running
notch filters on the active gyro
This commit is contained in:
Andrew Tridgell 2022-06-06 09:40:34 +10:00
parent a37f268dc6
commit d07761cfd9
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/Filter/NotchFilter.cpp
View File

@ -74,7 +74,7 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
template <class T>
T NotchFilter<T>::apply(const T &sample)
{
if (!initialised) {
if (!initialised || need_reset) {
// if we have not been initialised when return the input
// sample as output and update delayed samples
ntchsig2 = ntchsig1;
@ -82,6 +82,7 @@ T NotchFilter<T>::apply(const T &sample)
ntchsig = sample;
signal2 = signal1;
signal1 = sample;
need_reset = false;
return sample;
}
ntchsig2 = ntchsig1;
@ -96,8 +97,7 @@ T NotchFilter<T>::apply(const T &sample)
template <class T>
void NotchFilter<T>::reset()
{
ntchsig2 = ntchsig1 = T();
signal2 = signal1 = T();
need_reset = true;
}
/*

2
libraries/Filter/NotchFilter.h
View File

@ -40,7 +40,7 @@ public:
private:
bool initialised;
bool initialised, need_reset;
float b0, b1, b2, a1, a2, a0_inv;
T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
};