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

Merge pull request #11123 from KarateBrot/dynNotchGainFix 

Fix dynamic notch PT1 gain
This commit is contained in:
haslinghuis 2021-12-18 00:13:53 +01:00 committed by GitHub
parent cc2b28a290 ee6f256d8f
commit bc0c8ce0da
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/main/flight/dyn_notch_filter.c
View File

@ -151,7 +151,7 @@ static FAST_DATA_ZERO_INIT float sdftResolutionHz;
static FAST_DATA_ZERO_INIT int sdftStartBin;
static FAST_DATA_ZERO_INIT int sdftEndBin;
static FAST_DATA_ZERO_INIT float sdftMeanSq;
static FAST_DATA_ZERO_INIT float gain;
static FAST_DATA_ZERO_INIT float pt1LooptimeS;
void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeUs)
@ -186,7 +186,7 @@ void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeU
sdftResolutionHz = sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 18.5hz per bin at 8k and 600Hz maxHz
sdftStartBin = MAX(2, dynNotch.minHz / sdftResolutionHz + 0.5f); // can't use bin 0 because it is DC.
sdftEndBin = MIN(SDFT_BIN_COUNT - 1, dynNotch.maxHz / sdftResolutionHz + 0.5f); // can't use more than SDFT_BIN_COUNT bins.
gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / looprateHz); // minimum PT1 k value
pt1LooptimeS = DYN_NOTCH_CALC_TICKS / looprateHz;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, sampleCount);
@ -342,12 +342,12 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
// Convert bin to frequency: freq = bin * binResoultion (bin 0 is 0Hz)
const float centerFreq = constrainf(meanBin * sdftResolutionHz, dynNotch.minHz, dynNotch.maxHz);
// PT1 style smoothing moves notch center freqs rapidly towards big peaks and slowly away, up to 8x faster
// DYN_NOTCH_SMOOTH_HZ = 4 & gainMultiplier = 1 .. 8 => PT1 -3dB cutoff frequency = 4Hz .. 41Hz
const float gainMultiplier = constrainf(peaks[p].value / sdftMeanSq, 1.0f, 8.0f);
// PT1 style smoothing moves notch center freqs rapidly towards big peaks and slowly away, up to 10x faster
const float cutoffMult = constrainf(peaks[p].value / sdftMeanSq, 1.0f, 10.0f);
const float gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ * cutoffMult, pt1LooptimeS); // dynamic PT1 k value
// Finally update notch center frequency p on current axis
dynNotch.centerFreq[state.axis][p] += gain * gainMultiplier * (centerFreq - dynNotch.centerFreq[state.axis][p]);
dynNotch.centerFreq[state.axis][p] += gain * (centerFreq - dynNotch.centerFreq[state.axis][p]);
}
}