Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Filter: constrain harmonic notch center frequency changes to be within a slew limit 

raise notch filter slew for smaller aircraft
ensure NotchFilter init() resets the center frequency
This commit is contained in:
Andy Piper 2022-09-17 15:53:41 +01:00 committed by Randy Mackay
parent c232cf3d4f
commit c468958e60
2 changed files with 18 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/Filter/NotchFilter.cpp
View File

@ -20,6 +20,10 @@
#include "NotchFilter.h" #include "NotchFilter.h"
const static float NOTCH_MAX_SLEW = 0.05f;
const static float NOTCH_MAX_SLEW_LOWER = 1.0f - NOTCH_MAX_SLEW;
const static float NOTCH_MAX_SLEW_UPPER = 1.0f / NOTCH_MAX_SLEW_LOWER;
/* /*
calculate the attenuation and quality factors of the filter calculate the attenuation and quality factors of the filter
*/ */
@ -43,6 +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;
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 {
@ -53,8 +58,16 @@ 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)
{ {
if ((center_freq_hz > 0.0) && (center_freq_hz < 0.5 * sample_freq_hz) && (Q > 0.0)) { float new_center_freq = center_freq_hz;
float omega = 2.0 * M_PI * center_freq_hz / sample_freq_hz;
// constrain the new center frequency by a percentage of the old frequency
if (initialised && !need_reset && !is_zero(_center_freq_hz)) {
new_center_freq = constrain_float(new_center_freq, _center_freq_hz * NOTCH_MAX_SLEW_LOWER,
_center_freq_hz * NOTCH_MAX_SLEW_UPPER);
}
if ((new_center_freq > 0.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) {
float omega = 2.0 * M_PI * new_center_freq / sample_freq_hz;
float alpha = sinf(omega) / (2 * Q); float alpha = sinf(omega) / (2 * Q);
b0 = 1.0 + alpha*sq(A); b0 = 1.0 + alpha*sq(A);
b1 = -2.0 * cosf(omega); b1 = -2.0 * cosf(omega);
@ -62,8 +75,10 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
a0_inv = 1.0/(1.0 + alpha); a0_inv = 1.0/(1.0 + alpha);
a1 = b1; a1 = b1;
a2 = 1.0 - alpha; a2 = 1.0 - alpha;
_center_freq_hz = new_center_freq;
initialised = true; initialised = true;
} else { } else {
// leave center_freq_hz at last value
initialised = false; initialised = false;
} }
} }

1
libraries/Filter/NotchFilter.h
View File

@ -42,6 +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;
T ntchsig, ntchsig1, ntchsig2, signal2, signal1; T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
}; };