diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2cc8139411..04e29f80f7 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1422,7 +1422,7 @@ static bool blackboxWriteSysinfo(void) #ifdef USE_GYRO_DATA_ANALYSE BLACKBOX_PRINT_HEADER_LINE("dyn_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz); BLACKBOX_PRINT_HEADER_LINE("dyn_notch_count", "%d", gyroConfig()->dyn_notch_count); - BLACKBOX_PRINT_HEADER_LINE("dyn_notch_bandwidth_hz", "%d", gyroConfig()->dyn_notch_bandwidth_hz); + BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q); BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz); #endif #ifdef USE_DSHOT_TELEMETRY diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index dbeae62459..893f2f8d24 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -662,7 +662,7 @@ const clivalue_t valueTable[] = { #endif #if defined(USE_GYRO_DATA_ANALYSE) { "dyn_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, DYN_NOTCH_COUNT_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_count) }, - { "dyn_notch_bandwidth_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_bandwidth_hz) }, + { "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) }, { "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 60, 250 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) }, { "dyn_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_max_hz) }, #endif diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index d12ee61918..c246566c7c 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -757,7 +757,7 @@ static CMS_Menu cmsx_menuFilterGlobal = { #ifdef USE_GYRO_DATA_ANALYSE static uint16_t dynFiltNotchMaxHz; static uint8_t dynFiltCount; -static uint16_t dynFiltBandwidthHz; +static uint16_t dynFiltNotchQ; static uint16_t dynFiltNotchMinHz; #endif #ifdef USE_DYN_LPF @@ -776,7 +776,7 @@ static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp) #ifdef USE_GYRO_DATA_ANALYSE dynFiltNotchMaxHz = gyroConfig()->dyn_notch_max_hz; dynFiltCount = gyroConfig()->dyn_notch_count; - dynFiltBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz; + dynFiltNotchQ = gyroConfig()->dyn_notch_q; dynFiltNotchMinHz = gyroConfig()->dyn_notch_min_hz; #endif #ifdef USE_DYN_LPF @@ -800,7 +800,7 @@ static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry #ifdef USE_GYRO_DATA_ANALYSE gyroConfigMutable()->dyn_notch_max_hz = dynFiltNotchMaxHz; gyroConfigMutable()->dyn_notch_count = dynFiltCount; - gyroConfigMutable()->dyn_notch_bandwidth_hz = dynFiltBandwidthHz; + gyroConfigMutable()->dyn_notch_q = dynFiltNotchQ; gyroConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz; #endif #ifdef USE_DYN_LPF @@ -822,7 +822,7 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] = #ifdef USE_GYRO_DATA_ANALYSE { "NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltCount, 1, DYN_NOTCH_COUNT_MAX, 1 }, 0 }, - { "NOTCH WIDTH HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltBandwidthHz, 1, 1000, 1 }, 0 }, + { "NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ, 1, 1000, 1 }, 0 }, { "NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz, 0, 1000, 1 }, 0 }, { "NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz, 0, 1000, 1 }, 0 }, #endif diff --git a/src/main/common/sdft.c b/src/main/common/sdft.c index 6c9081bc3c..2e61634f1c 100644 --- a/src/main/common/sdft.c +++ b/src/main/common/sdft.c @@ -35,13 +35,13 @@ static FAST_DATA_ZERO_INIT complex_t twiddle[SDFT_BIN_COUNT]; static void applySqrt(const sdft_t *sdft, float *data); -void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const uint8_t numBatches) +void sdftInit(sdft_t *sdft, const int startBin, const int endBin, const int numBatches) { if (!isInitialized) { rPowerN = powf(SDFT_R, SDFT_SAMPLE_SIZE); const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE; float phi = 0.0f; - for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) { + for (int i = 0; i < SDFT_BIN_COUNT; i++) { phi = c * i; twiddle[i] = SDFT_R * (cos_approx(phi) + _Complex_I * sin_approx(phi)); } @@ -54,47 +54,47 @@ void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const sdft->numBatches = numBatches; sdft->batchSize = (sdft->endBin - sdft->startBin + 1) / sdft->numBatches + 1; - for (uint8_t i = 0; i < SDFT_SAMPLE_SIZE; i++) { + for (int i = 0; i < SDFT_SAMPLE_SIZE; i++) { sdft->samples[i] = 0.0f; } - for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) { + for (int i = 0; i < SDFT_BIN_COUNT; i++) { sdft->data[i] = 0.0f; } } // Add new sample to frequency spectrum -FAST_CODE void sdftPush(sdft_t *sdft, const float *sample) +FAST_CODE void sdftPush(sdft_t *sdft, const float sample) { - const float delta = *sample - rPowerN * sdft->samples[sdft->idx]; + const float delta = sample - rPowerN * sdft->samples[sdft->idx]; - sdft->samples[sdft->idx] = *sample; + sdft->samples[sdft->idx] = sample; sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE; - for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) { + for (int i = sdft->startBin; i <= sdft->endBin; i++) { sdft->data[i] = twiddle[i] * (sdft->data[i] + delta); } } // Add new sample to frequency spectrum in parts -FAST_CODE void sdftPushBatch(sdft_t* sdft, const float *sample, const uint8_t *batchIdx) +FAST_CODE void sdftPushBatch(sdft_t* sdft, const float sample, const int batchIdx) { - const uint8_t batchStart = sdft->batchSize * *batchIdx; - uint8_t batchEnd = batchStart; + const int batchStart = sdft->batchSize * batchIdx; + int batchEnd = batchStart; - const float delta = *sample - rPowerN * sdft->samples[sdft->idx]; + const float delta = sample - rPowerN * sdft->samples[sdft->idx]; - if (*batchIdx == sdft->numBatches - 1) { - sdft->samples[sdft->idx] = *sample; + if (batchIdx == sdft->numBatches - 1) { + sdft->samples[sdft->idx] = sample; sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE; batchEnd += sdft->endBin - batchStart + 1; } else { batchEnd += sdft->batchSize; } - for (uint8_t i = batchStart; i < batchEnd; i++) { + for (int i = batchStart; i < batchEnd; i++) { sdft->data[i] = twiddle[i] * (sdft->data[i] + delta); } } @@ -106,7 +106,7 @@ FAST_CODE void sdftMagSq(const sdft_t *sdft, float *output) float re; float im; - for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) { + for (int i = sdft->startBin; i <= sdft->endBin; i++) { re = crealf(sdft->data[i]); im = cimagf(sdft->data[i]); output[i] = re * re + im * im; @@ -130,7 +130,7 @@ FAST_CODE void sdftWinSq(const sdft_t *sdft, float *output) float re; float im; - for (uint8_t i = (sdft->startBin + 1); i < sdft->endBin; i++) { + 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); @@ -150,7 +150,7 @@ FAST_CODE void sdftWindow(const sdft_t *sdft, float *output) // Apply square root to the whole sdft range static FAST_CODE void applySqrt(const sdft_t *sdft, float *data) { - for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) { + for (int i = sdft->startBin; i <= sdft->endBin; i++) { data[i] = sqrtf(data[i]); } } diff --git a/src/main/common/sdft.h b/src/main/common/sdft.h index aad349bf8a..35b1d435b8 100644 --- a/src/main/common/sdft.h +++ b/src/main/common/sdft.h @@ -33,21 +33,19 @@ typedef float complex complex_t; // Better readability for type "float complex" typedef struct sdft_s { - uint8_t idx; // circular buffer index - uint8_t startBin; - uint8_t endBin; - uint8_t batchSize; - uint8_t numBatches; + int idx; // circular buffer index + int startBin; + int endBin; + int batchSize; + 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 <= (uint8_t)-1, window_size_greater_than_underlying_type); - -void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const uint8_t numBatches); -void sdftPush(sdft_t *sdft, const float *sample); -void sdftPushBatch(sdft_t *sdft, const float *sample, const uint8_t *batchIdx); +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); void sdftMagSq(const sdft_t *sdft, float *output); void sdftMagnitude(const sdft_t *sdft, float *output); void sdftWinSq(const sdft_t *sdft, float *output); diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index c643f8de20..0ed47a34a4 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -29,7 +29,6 @@ */ #include -#include #include "platform.h" @@ -98,7 +97,7 @@ typedef enum { typedef struct peak_s { - uint8_t bin; + int bin; float value; } peak_t; @@ -106,27 +105,27 @@ typedef struct peak_s { static sdft_t FAST_DATA_ZERO_INIT sdft[XYZ_AXIS_COUNT]; static peak_t FAST_DATA_ZERO_INIT peaks[DYN_NOTCH_COUNT_MAX]; static float FAST_DATA_ZERO_INIT sdftData[SDFT_BIN_COUNT]; -static uint16_t FAST_DATA_ZERO_INIT sdftSampleRateHz; +static float FAST_DATA_ZERO_INIT sdftSampleRateHz; static float FAST_DATA_ZERO_INIT sdftResolutionHz; -static uint8_t FAST_DATA_ZERO_INIT sdftStartBin; -static uint8_t FAST_DATA_ZERO_INIT sdftEndBin; +static int FAST_DATA_ZERO_INIT sdftStartBin; +static int FAST_DATA_ZERO_INIT sdftEndBin; static float FAST_DATA_ZERO_INIT sdftMeanSq; -static uint16_t FAST_DATA_ZERO_INIT dynNotchBandwidthHz; -static uint16_t FAST_DATA_ZERO_INIT dynNotchMinHz; -static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxHz; +static float FAST_DATA_ZERO_INIT dynNotchQ; +static float FAST_DATA_ZERO_INIT dynNotchMinHz; +static float FAST_DATA_ZERO_INIT dynNotchMaxHz; static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxFFT; -static float FAST_DATA_ZERO_INIT smoothFactor; -static uint8_t FAST_DATA_ZERO_INIT numSamples; +static float FAST_DATA_ZERO_INIT gain; +static int FAST_DATA_ZERO_INIT numSamples; -void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) +void gyroDataAnalyseInit(gyroAnalyseState_t *state, const uint32_t targetLooptimeUs) { // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later - dynNotchBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz; + dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f; dynNotchMinHz = gyroConfig()->dyn_notch_min_hz; dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz); // gyroDataAnalyse() is running at targetLoopRateHz (which is PID loop rate aka. 1e6f/gyro.targetLooptimeUs) - const int32_t targetLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f); + const float targetLoopRateHz = 1.0f / targetLooptimeUs * 1e6f; numSamples = MAX(1, targetLoopRateHz / (2 * dynNotchMaxHz)); // 600hz, 8k looptime, 6.00 sdftSampleRateHz = targetLoopRateHz / numSamples; @@ -137,20 +136,20 @@ void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) // eg 1k, user max 600hz, int(1000/1200) = 1 (max(1,0.8333)) sdftSampleRateHz = 1000hz, range 500Hz // the upper limit of DN is always going to be the Nyquist frequency (= sampleRate / 2) - sdftResolutionHz = (float)sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 13.3hz per bin at 8k - sdftStartBin = MAX(2, lrintf(dynNotchMinHz / sdftResolutionHz + 0.5f)); // can't use bin 0 because it is DC. - sdftEndBin = MIN(SDFT_BIN_COUNT - 1, lrintf(dynNotchMaxHz / sdftResolutionHz + 0.5f)); // can't use more than SDFT_BIN_COUNT bins. - smoothFactor = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / (float)targetLoopRateHz); // minimum PT1 k value + sdftResolutionHz = sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 13.3hz per bin at 8k + sdftStartBin = MAX(2, dynNotchMinHz / sdftResolutionHz + 0.5f); // can't use bin 0 because it is DC. + sdftEndBin = MIN(SDFT_BIN_COUNT - 1, dynNotchMaxHz / sdftResolutionHz + 0.5f); // can't use more than SDFT_BIN_COUNT bins. + gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / targetLoopRateHz); // minimum PT1 k value - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples); } state->maxSampleCount = numSamples; state->maxSampleCountRcp = 1.0f / state->maxSampleCount; - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int p = 0; p < gyro.notchFilterDynCount; p++) { // any init value is fine, but evenly spreading centerFreqs across frequency range makes notch filters stick to peaks quicker state->centerFreq[axis][p] = (p + 0.5f) * (dynNotchMaxHz - dynNotchMinHz) / (float)gyro.notchFilterDynCount + dynNotchMinHz; } @@ -158,7 +157,7 @@ void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) } // Collect gyro data, to be downsampled and analysed in gyroDataAnalyse() function -void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample) +void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample) { state->oversampledGyroAccumulator[axis] += sample; } @@ -174,7 +173,7 @@ FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state) state->sampleCount = 0; // calculate mean value of accumulated samples - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { const float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp; state->downsampledGyroData[axis] = sample; if (axis == 0) { @@ -192,8 +191,8 @@ FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state) // 2us @ F722 // SDFT processing in batches to synchronize with incoming downsampled data - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - sdftPushBatch(&sdft[axis], &state->downsampledGyroData[axis], &state->sampleCount); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + sdftPushBatch(&sdft[axis], state->downsampledGyroData[axis], state->sampleCount); } state->sampleCount++; @@ -222,7 +221,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) // Calculate mean square over frequency range (= average power of vibrations) sdftMeanSq = 0.0f; - for (uint8_t 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) } sdftMeanSq /= sdftEndBin - sdftStartBin - 1; @@ -234,20 +233,20 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) case STEP_DETECT_PEAKS: // 6us @ F722 { // Get memory ready for new peak data on current axis - for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { + for (int p = 0; p < gyro.notchFilterDynCount; p++) { peaks[p].bin = 0; peaks[p].value = 0.0f; } // Search for N biggest peaks in frequency spectrum - for (uint8_t bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { + for (int bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { // Check if bin is peak if ((sdftData[bin] > sdftData[bin - 1]) && (sdftData[bin] > sdftData[bin + 1])) { // Check if peak is big enough to be one of N biggest peaks. // If so, insert peak and sort peaks in descending height order - for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { + for (int p = 0; p < gyro.notchFilterDynCount; p++) { if (sdftData[bin] > peaks[p].value) { - for (uint8_t k = gyro.notchFilterDynCount - 1; k > p; k--) { + for (int k = gyro.notchFilterDynCount - 1; k > p; k--) { peaks[k] = peaks[k - 1]; } peaks[p].bin = bin; @@ -260,8 +259,8 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) } // Sort N biggest peaks in ascending bin order (example: 3, 8, 25, 0, 0, ..., 0) - for (uint8_t p = gyro.notchFilterDynCount - 1; p > 0; p--) { - for (uint8_t k = 0; k < p; k++) { + for (int p = gyro.notchFilterDynCount - 1; p > 0; p--) { + for (int k = 0; k < p; k++) { // Swap peaks but ignore swapping void peaks (bin = 0). This leaves // void peaks at the end of peaks array without moving them if (peaks[k].bin > peaks[k + 1].bin && peaks[k + 1].bin != 0) { @@ -278,61 +277,44 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) } case STEP_CALC_FREQUENCIES: // 4us @ F722 { - for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { + for (int p = 0; p < gyro.notchFilterDynCount; p++) { // Only update state->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) { - // accumulate sdftSum and sdftWeightedSum from peak bin, and shoulder bins either side of peak - float squaredData = peaks[p].value; // peak data already squared (see sdftWinSq) - float sdftSum = squaredData; - float sdftWeightedSum = squaredData * peaks[p].bin; + float meanBin = peaks[p].bin; - // accumulate upper shoulder unless it would be sdftEndBin - uint8_t shoulderBin = peaks[p].bin + 1; - if (shoulderBin < sdftEndBin) { - squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq) - sdftSum += squaredData; - sdftWeightedSum += squaredData * shoulderBin; + // Height of peak bin (y1) and shoulder bins (y0, y2) + const float y0 = sdftData[peaks[p].bin - 1]; + const float y1 = sdftData[peaks[p].bin]; + const float y2 = sdftData[peaks[p].bin + 1]; + + // Estimate true peak position aka. meanBin (fit parabola y(x) over y0, y1 and y2, solve dy/dx=0 for x) + const float denom = 2.0f * (y0 - 2 * y1 + y2); + if (denom != 0.0f) { + meanBin += (y0 - y2) / denom; } - // accumulate lower shoulder unless lower shoulder would be bin 0 (DC) - if (peaks[p].bin > 1) { - shoulderBin = peaks[p].bin - 1; - squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq) - sdftSum += squaredData; - sdftWeightedSum += squaredData * shoulderBin; - } + // Convert bin to frequency: freq = bin * binResoultion (bin 0 is 0Hz) + const float centerFreq = constrainf(meanBin * sdftResolutionHz, dynNotchMinHz, dynNotchMaxHz); - // get centerFreq in Hz from weighted bins - float centerFreq = dynNotchMaxHz; - float sdftMeanBin = 0; + // 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); - if (sdftSum > 0) { - sdftMeanBin = (sdftWeightedSum / sdftSum); - centerFreq = sdftMeanBin * sdftResolutionHz; - centerFreq = constrainf(centerFreq, dynNotchMinHz, dynNotchMaxHz); - // In theory, the index points to the centre frequency of the bin. - // at 1333hz, bin widths are 13.3Hz, so bin 2 (26.7Hz) has the range 20Hz to 33.3Hz - // Rav feels that maybe centerFreq = (sdftMeanBin + 0.5) * sdftResolutionHz is better - // empirical checking shows that not adding 0.5 works better - - // PT1 style dynamic smoothing moves rapidly towards big peaks and slowly away, up to 8x faster - // DYN_NOTCH_SMOOTH_HZ = 4 & dynamicFactor = 1 .. 8 => PT1 -3dB cutoff frequency = 4Hz .. 41Hz - const float dynamicFactor = constrainf(peaks[p].value / sdftMeanSq, 1.0f, 8.0f); - state->centerFreq[state->updateAxis][p] += smoothFactor * dynamicFactor * (centerFreq - state->centerFreq[state->updateAxis][p]); - } + // Finally update notch center frequency p on current axis + state->centerFreq[state->updateAxis][p] += gain * gainMultiplier * (centerFreq - state->centerFreq[state->updateAxis][p]); } } if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) { - for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { + for (int p = 0; p < gyro.notchFilterDynCount; p++) { dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]); } } if (state->updateAxis == gyro.gyroDebugAxis) { - for (uint8_t p = 0; p < gyro.notchFilterDynCount && p < 3; p++) { + for (int p = 0; p < gyro.notchFilterDynCount && p < 3; p++) { DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(state->centerFreq[state->updateAxis][p])); } DEBUG_SET(DEBUG_DYN_LPF, 1, lrintf(state->centerFreq[state->updateAxis][0])); @@ -344,13 +326,10 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) } case STEP_UPDATE_FILTERS: // 7us @ F722 { - for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { + for (int p = 0; p < gyro.notchFilterDynCount; p++) { // 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) { - // Choose notch Q in such a way that notch bandwidth stays constant (improves prop wash handling) - float dynamicQ = state->centerFreq[state->updateAxis][p] / (float)dynNotchBandwidthHz; - dynamicQ = constrainf(dynamicQ, 2.0f, 10.0f); - biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynamicQ, FILTER_NOTCH); + biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH); } } diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h index b5e4246276..e6ab561449 100644 --- a/src/main/flight/gyroanalyse.h +++ b/src/main/flight/gyroanalyse.h @@ -20,6 +20,10 @@ #pragma once +#include + +#include "common/axis.h" + #define DYN_NOTCH_COUNT_MAX 5 typedef struct gyroAnalyseState_s { @@ -42,8 +46,8 @@ typedef struct gyroAnalyseState_s { } gyroAnalyseState_t; -void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs); -void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample); +void gyroDataAnalyseInit(gyroAnalyseState_t *state, const uint32_t targetLooptimeUs); +void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample); void gyroDataAnalyse(gyroAnalyseState_t *state); uint16_t getMaxFFT(void); void resetMaxFFT(void); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index e46ef0ec7a..232684dbe8 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1780,7 +1780,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) #if defined(USE_GYRO_DATA_ANALYSE) sbufWriteU8(dst, 0); // DEPRECATED 1.43: dyn_notch_range sbufWriteU8(dst, 0); // DEPRECATED 1.44: dyn_notch_width_percent - sbufWriteU16(dst, 0); // DEPRECATED 1.44: dyn_notch_q + sbufWriteU16(dst, gyroConfig()->dyn_notch_q); sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz); #else sbufWriteU8(dst, 0); @@ -1809,10 +1809,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) #endif #if defined(USE_GYRO_DATA_ANALYSE) sbufWriteU8(dst, gyroConfig()->dyn_notch_count); - sbufWriteU16(dst, gyroConfig()->dyn_notch_bandwidth_hz); #else sbufWriteU8(dst, 0); - sbufWriteU16(dst, 0); #endif break; @@ -2656,7 +2654,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, #if defined(USE_GYRO_DATA_ANALYSE) sbufReadU8(src); // DEPRECATED 1.43: dyn_notch_range sbufReadU8(src); // DEPRECATED 1.44: dyn_notch_width_percent - sbufReadU16(src); // DEPRECATED 1.44: dyn_notch_q + gyroConfigMutable()->dyn_notch_q = sbufReadU16(src); gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src); #else sbufReadU8(src); @@ -2680,7 +2678,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU16(src); #endif } - if (sbufBytesRemaining(src) >= 4) { + if (sbufBytesRemaining(src) >= 2) { // Added in MSP API 1.44 #if defined(USE_DYN_LPF) currentPidProfile->dyn_lpf_curve_expo = sbufReadU8(src); @@ -2689,10 +2687,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, #endif #if defined(USE_GYRO_DATA_ANALYSE) gyroConfigMutable()->dyn_notch_count = sbufReadU8(src); - gyroConfigMutable()->dyn_notch_bandwidth_hz = sbufReadU16(src); #else sbufReadU8(src); - sbufReadU16(src); #endif } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 3953090de3..b57f099d54 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -129,8 +129,8 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) gyroConfig->dyn_lpf_gyro_min_hz = DYN_LPF_GYRO_MIN_HZ_DEFAULT; gyroConfig->dyn_lpf_gyro_max_hz = DYN_LPF_GYRO_MAX_HZ_DEFAULT; gyroConfig->dyn_notch_max_hz = 600; - gyroConfig->dyn_notch_count = 1; - gyroConfig->dyn_notch_bandwidth_hz = 45; + gyroConfig->dyn_notch_count = 3; + gyroConfig->dyn_notch_q = 300; gyroConfig->dyn_notch_min_hz = 150; gyroConfig->gyro_filter_debug_axis = FD_ROLL; gyroConfig->dyn_lpf_curve_expo = 5; diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 2491c75471..8abcf2f7a8 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -200,7 +200,7 @@ typedef struct gyroConfig_s { uint16_t dyn_notch_max_hz; uint8_t dyn_notch_count; - uint16_t dyn_notch_bandwidth_hz; + uint16_t dyn_notch_q; uint16_t dyn_notch_min_hz; uint8_t gyro_filter_debug_axis;