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

Filter: Alter Notch filter formula to remove /0 and allow perfect notch.

Browse Source 
This formulation of the notch equations lets the user specify the depth of the Notch. The presence of a diveide by A prevents the gain going to zero and therefore achieving a perfect notch. It also provides the risk that a user may attempt to do this and cause a divide by zero error. This change adds the ability to achive a perfect notch and removes the possibility of a divide by zero.

Add Notch Filter parameter checking
This commit is contained in:
Leonard Hall 2019-09-01 10:55:36 +09:30 committed by Andrew Tridgell
parent 9b9fb0d593
commit 6f3be90147
1 changed files with 33 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/Filter/NotchFilter.cpp
View File

@ -20,9 +20,13 @@
*/ */
template <class T> template <class T>
void NotchFilter<T>::calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q) { void NotchFilter<T>::calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q) {
const float octaves = log2f(center_freq_hz / (center_freq_hz - bandwidth_hz / 2.0f)) * 2.0f;
A = powf(10, -attenuation_dB / 40.0f); A = powf(10, -attenuation_dB / 40.0f);
Q = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1.0f); if (center_freq_hz > 0.5 * bandwidth_hz) {
const float octaves = log2f(center_freq_hz / (center_freq_hz - bandwidth_hz / 2.0f)) * 2.0f;
Q = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1.0f);
} else {
Q = 0.0;
}
} }
/* /*
@ -31,26 +35,32 @@ void NotchFilter<T>::calculate_A_and_Q(float center_freq_hz, float bandwidth_hz,
template <class T> template <class T>
void NotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB) void NotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB)
{ {
// adjust the center frequency to be in the allowable range // check center frequency is in the allowable range
center_freq_hz = constrain_float(center_freq_hz, bandwidth_hz * 0.52f, sample_freq_hz * 0.48f); if ((center_freq_hz > 0.5 * bandwidth_hz) && (center_freq_hz < 0.5 * sample_freq_hz)) {
float A, Q;
float A, Q; 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 {
initialised = false;
}
} }
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)
{ {
float omega = 2.0 * M_PI * center_freq_hz / sample_freq_hz; if ((center_freq_hz > 0.0) && (center_freq_hz < 0.5 * sample_freq_hz) && (Q > 0.0)) {
float alpha = sinf(omega) / (2 * Q/A); float omega = 2.0 * M_PI * center_freq_hz / sample_freq_hz;
b0 = 1.0 + alpha*A; float alpha = sinf(omega) / (2 * Q);
b1 = -2.0 * cosf(omega); b0 = 1.0 + alpha*sq(A);
b2 = 1.0 - alpha*A; b1 = -2.0 * cosf(omega);
a0_inv = 1.0/(1.0 + alpha/A); b2 = 1.0 - alpha*sq(A);
a1 = -2.0 * cosf(omega); a0_inv = 1.0/(1.0 + alpha);
a2 = 1.0 - alpha/A; a1 = b1;
initialised = true; a2 = 1.0 - alpha;
initialised = true;
} else {
initialised = false;
}
} }
/* /*
@ -61,7 +71,12 @@ T NotchFilter<T>::apply(const T &sample)
{ {
if (!initialised) { if (!initialised) {
// if we have not been initialised when return the input // if we have not been initialised when return the input
// sample as output // sample as output and update delayed samples
ntchsig2 = ntchsig1;
ntchsig1 = ntchsig;
ntchsig = sample;
signal2 = signal1;
signal1 = sample;
return sample; return sample;
} }
ntchsig2 = ntchsig1; ntchsig2 = ntchsig1;