Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Merge pull request #11143 from KarateBrot/noiseFloor

Browse source 
Enhance SDFT noise floor
This commit is contained in:
haslinghuis 2021-12-22 09:15:26 +01:00 • committed by GitHub
commit d293e45f57
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 26 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

38
src/main/flight/dyn_notch_filter.c
View file

@ -150,7 +150,7 @@ static FAST_DATA_ZERO_INIT float sdftSampleRateHz;
static FAST_DATA_ZERO_INIT float sdftResolutionHz; static FAST_DATA_ZERO_INIT float sdftResolutionHz;
static FAST_DATA_ZERO_INIT int sdftStartBin; static FAST_DATA_ZERO_INIT int sdftStartBin;
static FAST_DATA_ZERO_INIT int sdftEndBin; static FAST_DATA_ZERO_INIT int sdftEndBin;
static FAST_DATA_ZERO_INIT float sdftMeanSq; static FAST_DATA_ZERO_INIT float sdftNoiseThreshold;
static FAST_DATA_ZERO_INIT float pt1LooptimeS; static FAST_DATA_ZERO_INIT float pt1LooptimeS;
@ -259,22 +259,21 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
switch (state.step) { switch (state.step) {
case STEP_WINDOW: // 6us @ F722 case STEP_WINDOW: // 4.1us (3-6us) @ F722
{ {
sdftWinSq(&sdft[state.axis], sdftData); sdftWinSq(&sdft[state.axis], sdftData);
// Calculate mean square over frequency range (= average power of vibrations) // Get total vibrational power in dyn notch range for noise floor estimate in STEP_CALC_FREQUENCIES
sdftMeanSq = 0.0f; sdftNoiseThreshold = 0.0f;
for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { // don't use startBin or endBin because they are not windowed properly for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { // don't use startBin or endBin because they are not windowed properly
sdftMeanSq += sdftData[bin]; // sdftData is already squared (see sdftWinSq) sdftNoiseThreshold += sdftData[bin]; // sdftData contains power spectral density
} }
sdftMeanSq /= sdftEndBin - sdftStartBin - 1;
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break; break;
} }
case STEP_DETECT_PEAKS: // 6us @ F722 case STEP_DETECT_PEAKS: // 5.5us (4-7us) @ F722
{ {
// Get memory ready for new peak data on current axis // Get memory ready for new peak data on current axis
for (int p = 0; p < dynNotch.count; p++) { for (int p = 0; p < dynNotch.count; p++) {
@ -319,12 +318,27 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
break; break;
} }
case STEP_CALC_FREQUENCIES: // 4us @ F722 case STEP_CALC_FREQUENCIES: // 4.0us (2-7us) @ F722
{ {
// Approximate noise floor (= average power spectral density in dyn notch range, excluding peaks)
int peakCount = 0;
for (int p = 0; p < dynNotch.count; p++) {
if (peaks[p].bin != 0) {
sdftNoiseThreshold -= 0.75f * sdftData[peaks[p].bin - 1];
sdftNoiseThreshold -= sdftData[peaks[p].bin];
sdftNoiseThreshold -= 0.75f * sdftData[peaks[p].bin + 1];
peakCount++;
}
}
sdftNoiseThreshold /= sdftEndBin - sdftStartBin - peakCount - 1;
// A noise threshold 2 times the noise floor prevents peak tracking being too sensitive to noise
sdftNoiseThreshold *= 2.0f;
for (int p = 0; p < dynNotch.count; p++) { for (int p = 0; p < dynNotch.count; p++) {
// Only update dynNotch.centerFreq if there is a peak (ignore void peaks) and if peak is above noise floor // Only update dynNotch.centerFreq if there is a peak (ignore void peaks) and if peak is above noise floor
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) { if (peaks[p].bin != 0 && peaks[p].value > sdftNoiseThreshold) {
float meanBin = peaks[p].bin; float meanBin = peaks[p].bin;
@ -343,7 +357,7 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
const float centerFreq = constrainf(meanBin * sdftResolutionHz, dynNotch.minHz, dynNotch.maxHz); 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 10x faster // 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 cutoffMult = constrainf(peaks[p].value / sdftNoiseThreshold, 1.0f, 10.0f);
const float gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ * cutoffMult, pt1LooptimeS); // dynamic PT1 k value const float gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ * cutoffMult, pt1LooptimeS); // dynamic PT1 k value
// Finally update notch center frequency p on current axis // Finally update notch center frequency p on current axis
@ -368,11 +382,11 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void)
break; break;
} }
case STEP_UPDATE_FILTERS: // 7us @ F722 case STEP_UPDATE_FILTERS: // 5.4us (2-9us) @ F722
{ {
for (int p = 0; p < dynNotch.count; p++) { for (int p = 0; p < dynNotch.count; p++) {
// Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step // Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) { if (peaks[p].bin != 0 && peaks[p].value > sdftNoiseThreshold) {
biquadFilterUpdate(&dynNotch.notch[state.axis][p], dynNotch.centerFreq[state.axis][p], dynNotch.looptimeUs, dynNotch.q, FILTER_NOTCH, 1.0f); biquadFilterUpdate(&dynNotch.notch[state.axis][p], dynNotch.centerFreq[state.axis][p], dynNotch.looptimeUs, dynNotch.q, FILTER_NOTCH, 1.0f);
} }
} }