From 7fff10d536146f6de6971b7b5d84a342c3453329 Mon Sep 17 00:00:00 2001 From: Jan Post Date: Sat, 31 Dec 2022 12:36:24 +0100 Subject: [PATCH] Improved SDFT windowing (#12117) Proper SDFT windowing --- src/main/common/sdft.c | 25 +++++++++++++++++++++---- src/main/common/sdft.h | 8 +++++--- src/main/flight/dyn_notch_filter.c | 10 +++++----- 3 files changed, 31 insertions(+), 12 deletions(-) diff --git a/src/main/common/sdft.c b/src/main/common/sdft.c index 10dc861a6..6d2384261 100644 --- a/src/main/common/sdft.c +++ b/src/main/common/sdft.c @@ -50,9 +50,8 @@ void sdftInit(sdft_t *sdft, const int startBin, const int endBin, const int numB sdft->idx = 0; - // Add 1 bin on either side outside of range (if possible) to get proper windowing up to range limits - sdft->startBin = constrain(startBin - 1, 0, SDFT_BIN_COUNT - 1); - sdft->endBin = constrain(endBin + 1, sdft->startBin, SDFT_BIN_COUNT - 1); + sdft->startBin = constrain(startBin, 0, SDFT_BIN_COUNT - 1); + sdft->endBin = constrain(endBin, sdft->startBin, SDFT_BIN_COUNT - 1); sdft->numBatches = MAX(numBatches, 1); sdft->batchSize = (sdft->endBin - sdft->startBin) / sdft->numBatches + 1; // batchSize = ceil(numBins / numBatches) @@ -82,7 +81,7 @@ FAST_CODE void sdftPush(sdft_t *sdft, const float sample) // Add new sample to frequency spectrum in parts -FAST_CODE void sdftPushBatch(sdft_t* sdft, const float sample, const int batchIdx) +FAST_CODE void sdftPushBatch(sdft_t *sdft, const float sample, const int batchIdx) { const int batchStart = sdft->batchSize * batchIdx; int batchEnd = batchStart; @@ -133,12 +132,30 @@ FAST_CODE void sdftWinSq(const sdft_t *sdft, float *output) float re; float im; + if (sdft->startBin == 0) { + val = sdft->data[sdft->startBin] - sdft->data[sdft->startBin + 1]; + } else { + val = sdft->data[sdft->startBin] - 0.5f * (sdft->data[sdft->startBin - 1] + sdft->data[sdft->startBin + 1]); + } + re = crealf(val); + im = cimagf(val); + output[sdft->startBin] = re * re + im * im; + for (int i = (sdft->startBin + 1); i < sdft->endBin; i++) { val = sdft->data[i] - 0.5f * (sdft->data[i - 1] + sdft->data[i + 1]); // multiply by 2 to save one multiplication re = crealf(val); im = cimagf(val); output[i] = re * re + im * im; } + + if (sdft->endBin == SDFT_BIN_COUNT - 1) { + val = sdft->data[sdft->endBin] - sdft->data[sdft->endBin - 1]; + } else { + val = sdft->data[sdft->endBin] - 0.5f * (sdft->data[sdft->endBin - 1] + sdft->data[sdft->endBin + 1]); + } + re = crealf(val); + im = cimagf(val); + output[sdft->endBin] = re * re + im * im; } diff --git a/src/main/common/sdft.h b/src/main/common/sdft.h index 35b1d435b..7be3be918 100644 --- a/src/main/common/sdft.h +++ b/src/main/common/sdft.h @@ -23,16 +23,16 @@ #pragma once -#include #include #undef I // avoid collision of imaginary unit I with variable I in pid.h typedef float complex complex_t; // Better readability for type "float complex" +#include "common/utils.h" + #define SDFT_SAMPLE_SIZE 72 #define SDFT_BIN_COUNT (SDFT_SAMPLE_SIZE / 2) typedef struct sdft_s { - int idx; // circular buffer index int startBin; int endBin; @@ -40,9 +40,11 @@ typedef struct sdft_s { int numBatches; float samples[SDFT_SAMPLE_SIZE]; // circular buffer complex_t data[SDFT_BIN_COUNT]; // complex frequency spectrum - } sdft_t; +STATIC_ASSERT(SDFT_SAMPLE_SIZE % 2 == 0, sdft_sample_size_not_even); +STATIC_ASSERT(SDFT_BIN_COUNT >= 2, sdft_bin_count_too_small); + void sdftInit(sdft_t *sdft, const int startBin, const int endBin, const int numBatches); void sdftPush(sdft_t *sdft, const float sample); void sdftPushBatch(sdft_t *sdft, const float sample, const int batchIdx); diff --git a/src/main/flight/dyn_notch_filter.c b/src/main/flight/dyn_notch_filter.c index 59b3a5da0..44cf780f8 100644 --- a/src/main/flight/dyn_notch_filter.c +++ b/src/main/flight/dyn_notch_filter.c @@ -184,7 +184,7 @@ void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeU // the upper limit of DN is always going to be the Nyquist frequency (= sampleRate / 2) sdftResolutionHz = sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 18.5hz per bin at 8k and 600Hz maxHz - sdftStartBin = MAX(2, lrintf(dynNotch.minHz / sdftResolutionHz)); // can't use bin 0 because it is DC. + sdftStartBin = MAX(1, lrintf(dynNotch.minHz / sdftResolutionHz)); // can't use bin 0 because it is DC. sdftEndBin = MIN(SDFT_BIN_COUNT - 1, lrintf(dynNotch.maxHz / sdftResolutionHz)); // can't use more than SDFT_BIN_COUNT bins. pt1LooptimeS = DYN_NOTCH_CALC_TICKS / looprateHz; @@ -265,8 +265,8 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void) // Get total vibrational power in dyn notch range for noise floor estimate in STEP_CALC_FREQUENCIES sdftNoiseThreshold = 0.0f; - for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { // don't use startBin or endBin because they are not windowed properly - sdftNoiseThreshold += sdftData[bin]; // sdftData contains power spectral density + for (int bin = sdftStartBin; bin <= sdftEndBin; bin++) { + sdftNoiseThreshold += sdftData[bin]; // sdftData contains power spectral density } DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); @@ -297,7 +297,7 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void) break; } } - bin++; // If bin is peak, next bin can't be peak => jump it + bin++; // If bin is peak, next bin can't be peak => skip it } } @@ -330,7 +330,7 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void) peakCount++; } } - sdftNoiseThreshold /= sdftEndBin - sdftStartBin - peakCount - 1; + sdftNoiseThreshold /= sdftEndBin - sdftStartBin - peakCount + 1; // A noise threshold 2 times the noise floor prevents peak tracking being too sensitive to noise sdftNoiseThreshold *= 2.0f; -- 2.11.4.GIT