aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Flter: notchfilter: remove unneeded value and pre-multiply for speed

Browse Source
This commit is contained in:
Iampete1 2023-07-08 02:28:32 +01:00 committed by Andrew Tridgell
parent 0b24dc239f
commit 3c6ddda5e8
3 changed files with 18 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/Filter/NotchFilter.cpp
View File

@ -77,9 +77,18 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
b0 = 1.0 + alpha*sq(A);
b1 = -2.0 * cosf(omega);
b2 = 1.0 - alpha*sq(A);
a0_inv = 1.0/(1.0 + alpha);
a1 = b1;
a2 = 1.0 - alpha;
const float a0_inv = 1.0/(1.0 + alpha);
// Pre-multiply to save runtime calc
b0 *= a0_inv;
b1 *= a0_inv;
b2 *= a0_inv;
a1 *= a0_inv;
a2 *= a0_inv;
_center_freq_hz = new_center_freq;
_sample_freq_hz = sample_freq_hz;
initialised = true;
@ -100,16 +109,17 @@ T NotchFilter<T>::apply(const T &sample)
// sample as output and update delayed samples
signal1 = sample;
signal2 = sample;
ntchsig = sample;
ntchsig1 = sample;
ntchsig2 = sample;
need_reset = false;
return sample;
}
T output = sample*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2;
ntchsig2 = ntchsig1;
ntchsig1 = ntchsig;
ntchsig = sample;
T output = (ntchsig*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2) * a0_inv;
ntchsig1 = sample;
signal2 = signal1;
signal1 = output;
return output;

4
libraries/Filter/NotchFilter.h
View File

@ -41,9 +41,9 @@ public:
protected:
bool initialised, need_reset;
float b0, b1, b2, a1, a2, a0_inv;
float b0, b1, b2, a1, a2;
float _center_freq_hz, _sample_freq_hz;
T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
T ntchsig1, ntchsig2, signal2, signal1;
};
/*

2
libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp
View File

@ -61,7 +61,7 @@ public:
hal.console->printf("NotchFilterFloat\n");
hal.console->printf("Sample rate: %.9f Hz, Center: %.9f Hz\n", _sample_freq_hz, _center_freq_hz);
hal.console->printf("Notch filter in the form: H(z) = (b0 + b1*z^-1 + b2*z^-2)/(a0 + a1*z^-1 + a2*z^-2)\n");
hal.console->printf("a0: %.9f, a1: %.9f, a2: %.9f, b0: %.9f, b1: %.9f, b2: %.9f\n", 1.0/a0_inv, a1, a2, b0, b1, b2);
hal.console->printf("a0: %.9f, a1: %.9f, a2: %.9f, b0: %.9f, b1: %.9f, b2: %.9f\n", 1.0, a1, a2, b0, b1, b2);
}
};