rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #10742 from KarateBrot/filter

This commit is contained in:
Michael Keller 2021-05-21 15:45:00 +12:00 committed by GitHub
parent 097d805308 3d78412fd1
commit 296b87fc6f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 49 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
src/main/common/filter.c
View File

@ -44,7 +44,7 @@ FAST_CODE float nullFilterApply(filter_t *filter, float input)
float pt1FilterGain(float f_cut, float dT) float pt1FilterGain(float f_cut, float dT)
{ {
float RC = 1 / ( 2 * M_PIf * f_cut); float RC = 1 / (2 * M_PIf * f_cut);
return dT / (RC + dT); return dT / (RC + dT);
} }
@ -70,9 +70,9 @@ FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
float pt2FilterGain(float f_cut, float dT) float pt2FilterGain(float f_cut, float dT)
{ {
const float order = 2.0f; const float order = 2.0f;
const float orderCutoffCorrection = (1 / sqrtf(powf(2, 1.0f / (order)) - 1)); const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1);
float RC = 1 / ( 2 * orderCutoffCorrection * M_PIf * f_cut); float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut);
//float RC = 1 / ( 2 * 1.553773974f * M_PIf * f_cut); // float RC = 1 / (2 * 1.553773974f * M_PIf * f_cut);
// where 1.553773974 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 2 // where 1.553773974 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 2
return dT / (RC + dT); return dT / (RC + dT);
} }
@ -101,9 +101,9 @@ FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input)
float pt3FilterGain(float f_cut, float dT) float pt3FilterGain(float f_cut, float dT)
{ {
const float order = 3.0f; const float order = 3.0f;
const float orderCutoffCorrection = (1 / sqrtf(powf(2, 1.0f / (order)) - 1)); const float orderCutoffCorrection = 1 / sqrtf(powf(2, 1.0f / order) - 1);
float RC = 1 / ( 2 * orderCutoffCorrection * M_PIf * f_cut); float RC = 1 / (2 * orderCutoffCorrection * M_PIf * f_cut);
// float RC = 1 / ( 2 * 1.961459177f * M_PIf * f_cut); // float RC = 1 / (2 * 1.961459177f * M_PIf * f_cut);
// where 1.961459177 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 3 // where 1.961459177 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 3
return dT / (RC + dT); return dT / (RC + dT);
} }
@ -157,11 +157,12 @@ FAST_CODE float slewFilterApply(slewFilter_t *filter, float input)
// get notch filter Q given center frequency (f0) and lower cutoff frequency (f1) // get notch filter Q given center frequency (f0) and lower cutoff frequency (f1)
// Q = f0 / (f2 - f1) ; f2 = f0^2 / f1 // Q = f0 / (f2 - f1) ; f2 = f0^2 / f1
float filterGetNotchQ(float centerFreq, float cutoffFreq) { float filterGetNotchQ(float centerFreq, float cutoffFreq)
{
return centerFreq * cutoffFreq / (centerFreq * centerFreq - cutoffFreq * cutoffFreq); return centerFreq * cutoffFreq / (centerFreq * centerFreq - cutoffFreq * cutoffFreq);
} }
/* sets up a biquad Filter */ /* sets up a biquad filter as a 2nd order butterworth LPF */
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate) void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
{ {
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
@ -169,49 +170,7 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{ {
// setup variables biquadFilterUpdate(filter, filterFreq, refreshRate, Q, filterType);
const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f;
const float sn = sin_approx(omega);
const float cs = cos_approx(omega);
const float alpha = sn / (2.0f * Q);
float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0;
switch (filterType) {
case FILTER_LPF:
// 2nd order Butterworth (with Q=1/sqrt(2)) / Butterworth biquad section with Q
// described in http://www.ti.com/lit/an/slaa447/slaa447.pdf
b0 = (1 - cs) * 0.5f;
b1 = 1 - cs;
b2 = (1 - cs) * 0.5f;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
break;
case FILTER_NOTCH:
b0 = 1;
b1 = -2 * cs;
b2 = 1;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
break;
case FILTER_BPF:
b0 = alpha;
b1 = 0;
b2 = -alpha;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
break;
}
// precompute the coefficients
filter->b0 = b0 / a0;
filter->b1 = b1 / a0;
filter->b2 = b2 / a0;
filter->a1 = a1 / a0;
filter->a2 = a2 / a0;
// zero initial samples // zero initial samples
filter->x1 = filter->x2 = 0; filter->x1 = filter->x2 = 0;
@ -220,19 +179,46 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{ {
// backup state // setup variables
float x1 = filter->x1; const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f;
float x2 = filter->x2; const float sn = sin_approx(omega);
float y1 = filter->y1; const float cs = cos_approx(omega);
float y2 = filter->y2; const float alpha = sn / (2.0f * Q);
biquadFilterInit(filter, filterFreq, refreshRate, Q, filterType); switch (filterType) {
case FILTER_LPF:
// 2nd order Butterworth (with Q=1/sqrt(2)) / Butterworth biquad section with Q
// described in http://www.ti.com/lit/an/slaa447/slaa447.pdf
filter->b1 = 1 - cs;
filter->b0 = filter->b1 * 0.5f;
filter->b2 = filter->b0;
filter->a1 = -2 * cs;
filter->a2 = 1 - alpha;
break;
case FILTER_NOTCH:
filter->b0 = 1;
filter->b1 = -2 * cs;
filter->b2 = 1;
filter->a1 = filter->b1;
filter->a2 = 1 - alpha;
break;
case FILTER_BPF:
filter->b0 = alpha;
filter->b1 = 0;
filter->b2 = -alpha;
filter->a1 = -2 * cs;
filter->a2 = 1 - alpha;
break;
}
// restore state const float a0 = 1 + alpha;
filter->x1 = x1;
filter->x2 = x2; // precompute the coefficients
filter->y1 = y1; filter->b0 /= a0;
filter->y2 = y2; filter->b1 /= a0;
filter->b2 /= a0;
filter->a1 /= a0;
filter->a2 /= a0;
} }
FAST_CODE void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate) FAST_CODE void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)

2
src/main/common/maths.h
View File

@ -34,8 +34,6 @@
// Use floating point M_PI instead explicitly. // Use floating point M_PI instead explicitly.
#define M_PIf 3.14159265358979323846f #define M_PIf 3.14159265358979323846f
#define M_EULERf 2.71828182845904523536f #define M_EULERf 2.71828182845904523536f
#define M_SQRT2f 1.41421356237309504880f
#define M_LN2f 0.69314718055994530942f
#define RAD (M_PIf / 180.0f) #define RAD (M_PIf / 180.0f)
#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10) #define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)