Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00
Code Issues Wiki Activity

Fixed notch Q calculation to be dimensionless as it should (#5529)

Browse source 
* Fixed notch Q calculation to be dimensionless as it should

* Get rid of one division and leverage multiply-accumulate instr
This commit is contained in:
Andrey Mironov 2018-03-24 20:27:39 +03:00 • committed by GitHub
parent 0c7ae49464
commit 172642383d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 4 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

7
src/main/common/filter.c
View file

@ -86,11 +86,8 @@ 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(uint16_t centerFreq, uint16_t cutoff) { float filterGetNotchQ(float centerFreq, float cutoffFreq) {
const float f0sq = (float)centerFreq * centerFreq; return centerFreq * cutoffFreq / (centerFreq * centerFreq - cutoffFreq * cutoffFreq);
const float f1 = cutoff;
return (f1 * f0sq) / (f0sq - f1 * f1);
} }
/* sets up a biquad Filter */ /* sets up a biquad Filter */

5
src/main/common/filter.h
View file

@ -116,7 +116,7 @@ int biquadFilterLpfCascadeInit(biquadFilter_t *sections, int order, float filter
float biquadFilterApplyDF1(biquadFilter_t *filter, float input); float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input); float biquadFilterApply(biquadFilter_t *filter, float input);
float biquadCascadeFilterApply(biquadFilterCascade_t *filter, float input); float biquadCascadeFilterApply(biquadFilterCascade_t *filter, float input);
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff); float filterGetNotchQ(float centerFreq, float cutoffFreq);
void biquadRCFIR2FilterInit(biquadFilter_t *filter, float k); void biquadRCFIR2FilterInit(biquadFilter_t *filter, float k);
@ -126,9 +126,6 @@ float fastKalmanUpdate(fastKalman_t *filter, float input);
void lmaSmoothingInit(laggedMovingAverage_t *filter, uint8_t windowSize, float weight); void lmaSmoothingInit(laggedMovingAverage_t *filter, uint8_t windowSize, float weight);
float lmaSmoothingUpdate(laggedMovingAverage_t *filter, float input); float lmaSmoothingUpdate(laggedMovingAverage_t *filter, float input);
// not exactly correct, but very very close and much much faster
#define filterGetNotchQApprox(centerFreq, cutoff) ((float)(cutoff * centerFreq) / ((float)(centerFreq - cutoff) * (float)(centerFreq + cutoff)))
float pt1FilterGain(uint16_t f_cut, float dT); float pt1FilterGain(uint16_t f_cut, float dT);
void pt1FilterInit(pt1Filter_t *filter, float k); void pt1FilterInit(pt1Filter_t *filter, float k);
float pt1FilterApply(pt1Filter_t *filter, float input); float pt1FilterApply(pt1Filter_t *filter, float input);

2
src/main/sensors/gyroanalyse.c
View file

@ -281,7 +281,7 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
// 7us // 7us
// calculate new filter coefficients // calculate new filter coefficients
float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF); float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF);
float notchQ = filterGetNotchQApprox(fftResult[axis].centerFreq, cutoffFreq); float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq);
biquadFilterUpdate(&notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterUpdate(&notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);