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

Filter: optimize notch filter frequency updates when the requested frequency has not changed

Browse Source
This commit is contained in:
Andy Piper 2022-09-21 10:45:42 +01:00 committed by Andrew Tridgell
parent e9918306fd
commit 1fe7fb8f35
2 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/Filter/NotchFilter.cpp
View File

@ -47,7 +47,7 @@ void NotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float band
// check center frequency is in the allowable range // check center frequency is in the allowable range
if ((center_freq_hz > 0.5 * bandwidth_hz) && (center_freq_hz < 0.5 * sample_freq_hz)) { if ((center_freq_hz > 0.5 * bandwidth_hz) && (center_freq_hz < 0.5 * sample_freq_hz)) {
float A, Q; float A, Q;
_center_freq_hz = center_freq_hz; initialised = false; // force center frequency change
calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, A, Q); calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, A, Q);
init_with_A_and_Q(sample_freq_hz, center_freq_hz, A, Q); init_with_A_and_Q(sample_freq_hz, center_freq_hz, A, Q);
} else { } else {
@ -58,6 +58,11 @@ void NotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float band
template <class T> template <class T>
void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q) void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q)
{ {
// don't update if no updates required
if (initialised && is_equal(center_freq_hz, _center_freq_hz) && is_equal(sample_freq_hz, _sample_freq_hz)) {
return;
}
float new_center_freq = center_freq_hz; float new_center_freq = center_freq_hz;
// constrain the new center frequency by a percentage of the old frequency // constrain the new center frequency by a percentage of the old frequency
@ -76,6 +81,7 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
a1 = b1; a1 = b1;
a2 = 1.0 - alpha; a2 = 1.0 - alpha;
_center_freq_hz = new_center_freq; _center_freq_hz = new_center_freq;
_sample_freq_hz = sample_freq_hz;
initialised = true; initialised = true;
} else { } else {
// leave center_freq_hz at last value // leave center_freq_hz at last value

2
libraries/Filter/NotchFilter.h
View File

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