From d02af7334c7f5caf5e8dc412fa58b893fb0d3fcd Mon Sep 17 00:00:00 2001 From: KarateBrot Date: Tue, 22 Sep 2020 18:15:06 +0200 Subject: [PATCH] Added tracking of multiple dynamic notches per axis and replaced FFT with SDFT --- make/source.mk | 11 +- src/main/blackbox/blackbox.c | 4 +- src/main/cli/settings.c | 4 +- src/main/cms/cms_menu_firmware.c | 1 + src/main/cms/cms_menu_imu.c | 16 +- src/main/common/filter.c | 6 +- src/main/common/maths.h | 9 +- src/main/common/sdft.c | 156 ++++++++++ src/main/common/sdft.h | 54 ++++ src/main/flight/gyroanalyse.c | 458 +++++++++++++--------------- src/main/flight/gyroanalyse.h | 24 +- src/main/msp/msp.c | 28 +- src/main/sensors/gyro.c | 8 +- src/main/sensors/gyro.h | 13 +- src/main/sensors/gyro_filter_impl.c | 25 +- src/main/sensors/gyro_init.c | 13 +- 16 files changed, 504 insertions(+), 326 deletions(-) create mode 100644 src/main/common/sdft.c create mode 100644 src/main/common/sdft.h diff --git a/make/source.mk b/make/source.mk index cef0117e7..adae17283 100644 --- a/make/source.mk +++ b/make/source.mk @@ -226,6 +226,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ common/encoding.c \ common/filter.c \ common/maths.c \ + common/sdft.c \ common/typeconversion.c \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_mpu3050.c \ @@ -397,16 +398,6 @@ ifneq ($(DSP_LIB),) INCLUDE_DIRS += $(DSP_LIB)/Include -SRC += $(DSP_LIB)/Source/BasicMathFunctions/arm_mult_f32.c -SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_f32.c -SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_f32.c -SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c -SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_radix8_f32.c -SRC += $(DSP_LIB)/Source/CommonTables/arm_common_tables.c - -SRC += $(DSP_LIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c -SRC += $(DSP_LIB)/Source/StatisticsFunctions/arm_max_f32.c - SRC += $(wildcard $(DSP_LIB)/Source/*/*.S) endif diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f3c67991e..27f33eb20 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1419,8 +1419,8 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("gyro_to_use", "%d", gyroConfig()->gyro_to_use); #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_width_percent", "%d", gyroConfig()->dyn_notch_width_percent); - BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q); + 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_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 bad004424..270254875 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -679,8 +679,8 @@ const clivalue_t valueTable[] = { { "gyro_to_use", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, #endif #if defined(USE_GYRO_DATA_ANALYSE) - { "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) }, - { "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) }, + { "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_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_firmware.c b/src/main/cms/cms_menu_firmware.c index b4ec866ba..316de16fc 100644 --- a/src/main/cms/cms_menu_firmware.c +++ b/src/main/cms/cms_menu_firmware.c @@ -25,6 +25,7 @@ #include #include +#include #include "platform.h" diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index c3a559fa2..a1ba59966 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -752,8 +752,8 @@ static CMS_Menu cmsx_menuFilterGlobal = { #ifdef USE_GYRO_DATA_ANALYSE static uint16_t dynFiltNotchMaxHz; -static uint8_t dynFiltWidthPercent; -static uint16_t dynFiltNotchQ; +static uint8_t dynFiltCount; +static uint16_t dynFiltBandwidthHz; static uint16_t dynFiltNotchMinHz; #endif #ifdef USE_DYN_LPF @@ -771,8 +771,8 @@ static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp) #ifdef USE_GYRO_DATA_ANALYSE dynFiltNotchMaxHz = gyroConfig()->dyn_notch_max_hz; - dynFiltWidthPercent = gyroConfig()->dyn_notch_width_percent; - dynFiltNotchQ = gyroConfig()->dyn_notch_q; + dynFiltCount = gyroConfig()->dyn_notch_count; + dynFiltBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz; dynFiltNotchMinHz = gyroConfig()->dyn_notch_min_hz; #endif #ifdef USE_DYN_LPF @@ -795,8 +795,8 @@ 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_width_percent = dynFiltWidthPercent; - gyroConfigMutable()->dyn_notch_q = dynFiltNotchQ; + gyroConfigMutable()->dyn_notch_count = dynFiltCount; + gyroConfigMutable()->dyn_notch_bandwidth_hz = dynFiltBandwidthHz; gyroConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz; #endif #ifdef USE_DYN_LPF @@ -817,8 +817,8 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] = { "-- DYN FILT --", OME_Label, NULL, NULL, 0 }, #ifdef USE_GYRO_DATA_ANALYSE - { "NOTCH WIDTH %", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltWidthPercent, 0, 20, 1 }, 0 }, - { "NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ, 0, 1000, 1 }, 0 }, + { "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 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/filter.c b/src/main/common/filter.c index cbbc94697..4ab5f9abc 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -29,8 +29,6 @@ #include "common/maths.h" #include "common/utils.h" -#define M_LN2_FLOAT 0.69314718055994530942f -#define M_PI_FLOAT 3.14159265358979323846f #define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - 2nd order butterworth*/ // NULL filter @@ -46,7 +44,7 @@ FAST_CODE float nullFilterApply(filter_t *filter, float input) float pt1FilterGain(float f_cut, float dT) { - float RC = 1 / ( 2 * M_PI_FLOAT * f_cut); + float RC = 1 / ( 2 * M_PIf * f_cut); return dT / (RC + dT); } @@ -107,7 +105,7 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) { // setup variables - const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f; + const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f; const float sn = sin_approx(omega); const float cs = cos_approx(omega); const float alpha = sn / (2.0f * Q); diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 16d3a0033..94306617f 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -29,10 +29,13 @@ // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations #define FAST_MATH // order 9 approximation -#define VERY_FAST_MATH // order 7 approximation +#define VERY_FAST_MATH // order 7 approximation // Use floating point M_PI instead explicitly. #define M_PIf 3.14159265358979323846f +#define M_EULERf 2.71828182845904523536f +#define M_SQRT2f 1.41421356237309504880f +#define M_LN2f 0.69314718055994530942f #define RAD (M_PIf / 180.0f) #define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10) @@ -137,8 +140,8 @@ float exp_approx(float val); float log_approx(float val); float pow_approx(float a, float b); #else -#define sin_approx(x) sinf(x) -#define cos_approx(x) cosf(x) +#define sin_approx(x) sinf(x) +#define cos_approx(x) cosf(x) #define atan2_approx(y,x) atan2f(y,x) #define acos_approx(x) acosf(x) #define tan_approx(x) tanf(x) diff --git a/src/main/common/sdft.c b/src/main/common/sdft.c new file mode 100644 index 000000000..eafc0776e --- /dev/null +++ b/src/main/common/sdft.c @@ -0,0 +1,156 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#include "common/maths.h" +#include "common/sdft.h" + +#define SDFT_R 0.9999f // damping factor for guaranteed SDFT stability (r < 1.0f) + +static FAST_DATA_ZERO_INIT float rPowerN; // SDFT_R to the power of SDFT_SAMPLE_SIZE +static FAST_DATA_ZERO_INIT bool isInitialized; +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) +{ + if (!isInitialized) { + rPowerN = powerf(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++) { + phi = c * i; + twiddle[i] = SDFT_R * (cos_approx(phi) + _Complex_I * sin_approx(phi)); + } + isInitialized = true; + } + + sdft->idx = 0; + sdft->startBin = startBin; + sdft->endBin = endBin; + sdft->numBatches = numBatches; + sdft->batchSize = (sdft->endBin - sdft->startBin + 1) / sdft->numBatches + 1; + + for (uint8_t i = 0; i < SDFT_SAMPLE_SIZE; i++) { + sdft->samples[i] = 0.0f; + } + + for (uint8_t 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) +{ + const float delta = *sample - rPowerN * sdft->samples[sdft->idx]; + + sdft->samples[sdft->idx] = *sample; + sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE; + + for (uint8_t 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) +{ + const uint8_t batchStart = sdft->batchSize * *batchIdx; + uint8_t batchEnd = batchStart; + + const float delta = *sample - rPowerN * sdft->samples[sdft->idx]; + + 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++) { + sdft->data[i] = twiddle[i] * (sdft->data[i] + delta); + } +} + + +// Get squared magnitude of frequency spectrum +FAST_CODE void sdftMagSq(const sdft_t *sdft, float *output) +{ + float re; + float im; + + for (uint8_t i = sdft->startBin; i <= sdft->endBin; i++) { + re = crealf(sdft->data[i]); + im = cimagf(sdft->data[i]); + output[i] = re * re + im * im; + } +} + + +// Get magnitude of frequency spectrum (slower) +FAST_CODE void sdftMagnitude(const sdft_t *sdft, float *output) +{ + sdftMagSq(sdft, output); + applySqrt(sdft, output); +} + + +// Get squared magnitude of frequency spectrum with Hann window applied +// Hann window in frequency domain: X[k] = -0.25 * X[k-1] +0.5 * X[k] -0.25 * X[k+1] +FAST_CODE void sdftWinSq(const sdft_t *sdft, float *output) +{ + complex_t val; + float re; + float im; + + for (uint8_t 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; + } +} + + +// Get magnitude of frequency spectrum with Hann window applied (slower) +FAST_CODE void sdftWindow(const sdft_t *sdft, float *output) +{ + sdftWinSq(sdft, output); + applySqrt(sdft, 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++) { + data[i] = sqrtf(data[i]); + } +} diff --git a/src/main/common/sdft.h b/src/main/common/sdft.h new file mode 100644 index 000000000..aad349bf8 --- /dev/null +++ b/src/main/common/sdft.h @@ -0,0 +1,54 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + + // Implementation of a Sliding Discrete Fourier Transform (SDFT). + // Complexity for calculating frequency spectrum with N bins is O(N). + +#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" + +#define SDFT_SAMPLE_SIZE 72 +#define SDFT_BIN_COUNT (SDFT_SAMPLE_SIZE / 2) + +typedef struct sdft_s { + + uint8_t idx; // circular buffer index + uint8_t startBin; + uint8_t endBin; + uint8_t batchSize; + uint8_t 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 sdftMagSq(const sdft_t *sdft, float *output); +void sdftMagnitude(const sdft_t *sdft, float *output); +void sdftWinSq(const sdft_t *sdft, float *output); +void sdftWindow(const sdft_t *sdft, float *output); diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index d064855c6..749c263cf 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -19,10 +19,16 @@ */ /* original work by Rav + * * 2018_07 updated by ctzsnooze to post filter, wider Q, different peak detection * coding assistance and advice from DieHertz, Rav, eTracer * test pilots icr4sh, UAV Tech, Flint723 + * + * 2021_02 updated by KarateBrot: switched FFT with SDFT, multiple notches per axis + * test pilots: Sugar K, bizmar */ + +#include #include #include "platform.h" @@ -32,6 +38,7 @@ #include "common/filter.h" #include "common/maths.h" +#include "common/sdft.h" #include "common/time.h" #include "common/utils.h" @@ -44,62 +51,72 @@ #include "gyroanalyse.h" -// FFT_WINDOW_SIZE defaults to 32 (gyroanalyse.h) -// We get 16 frequency bins from 32 consecutive data values +// SDFT_SAMPLE_SIZE defaults to 72 (common/sdft.h). +// We get 36 frequency bins from 72 consecutive data values, called SDFT_BIN_COUNT (common/sdft.h) // Bin 0 is DC and can't be used. -// Only bins 1 to 15 are usable. +// Only bins 1 to 35 are usable. -// A gyro sample is collected every gyro loop +// A gyro sample is collected every PID loop. // maxSampleCount recent gyro values are accumulated and averaged -// to ensure that 32 samples are collected at the right rate for the required FFT bandwidth +// to ensure that 72 samples are collected at the right rate for the required SDFT bandwidth. -// For an 8k gyro loop, at default 600hz max, 6 sequential gyro data points are averaged, FFT runs 1333Hz. -// Upper limit of FFT is half that frequency, eg 666Hz by default. -// At 8k, if user sets a max of 300Hz, int(8000/600) = 13, fftSamplingRateHz = 615Hz, range 307Hz +// For an 8k PID loop, at default 600hz max, 6 sequential gyro data points are averaged, SDFT runs 1333Hz. +// Upper limit of SDFT is half that frequency, eg 666Hz by default. +// At 8k, if user sets a max of 300Hz, int(8000/600) = 13, sdftSampleRateHz = 615Hz, range 307Hz. // Note that lower max requires more samples to be averaged, increasing precision but taking longer to get enough samples. -// For Bosch at 3200Hz gyro, max of 600, int(3200/1200) = 2, fftSamplingRateHz = 1600, range to 800hz -// For Bosch on XClass, better to set a max of 300, int(3200/600) = 5, fftSamplingRateHz = 640, range to 320Hz -// -// When sampleCount reaches maxSampleCount, the averaged gyro value is put into the circular buffer of 32 samples -// At 8k, with 600Hz max, maxSampleCount = 6, this happens every 6 * 0.125us, or every 0.75ms -// Hence to completely replace all 32 samples of the FFT input buffer with clean new data takes 24ms +// For Bosch at 3200Hz gyro, max of 600, int(3200/1200) = 2, sdftSampleRateHz = 1600, range to 800hz. +// For Bosch on XClass, better to set a max of 300, int(3200/600) = 5, sdftSampleRateHz = 640, range to 320Hz. -// The FFT code is split into steps. It takes 4 gyro loops to calculate the FFT for one axis -// (gyroDataAnalyseUpdate has 8 steps, but only four breaks) -// Since there are three axes, it takes 12 gyro loops to completely update all axes. +// When sampleCount reaches maxSampleCount, the averaged gyro value is put into the corresponding SDFT. +// At 8k, with 600Hz max, maxSampleCount = 6, this happens every 6 * 0.125us, or every 0.75ms. +// Hence to completely replace all 72 samples of the SDFT input buffer with clean new data takes 54ms. + +// The SDFT code is split into steps. It takes 4 PID loops to calculate the SDFT, track peaks and update the filters for one axis. +// Since there are three axes, it takes 12 PID loops to completely update all axes. // At 8k, any one axis gets updated at 8000 / 12 or 666hz or every 1.5ms -// In this time, 2 points in the FFT buffer will have changed. -// At 4k, it takes twice as long to update an axis, i.e. each axis updates only every 3ms +// In this time, 2 points in the SDFT buffer will have changed. +// At 4k, it takes twice as long to update an axis, i.e. each axis updates only every 3ms. // Four points in the buffer will have changed in that time, and each point will be the average of three samples. -// Hence output jitter at 4k is about four times worse than at 8k. At 2k output jitter is quite bad. +// Hence output jitter at 4k is about four times worse than at 8k. At 2k output jitter is quite bad. -// The Hanning window step loads gyro data (32 data points) for one axis from the circular buffer into fftData[i] -// and applies the hanning window to the edge values -// Calculation steps 1 and 2 then calculate the fft output (32 data points) and put that back into the same fftData[i] array. -// We then use fftData[i] array for frequency centre calculations for that axis +// Each SDFT output bin has width sdftSampleRateHz/72, ie 18.5Hz per bin at 1333Hz. +// Usable bandwidth is half this, ie 666Hz if sdftSampleRateHz is 1333Hz, i.e. bin 1 is 18.5Hz, bin 2 is 37.0Hz etc. -// Each FFT output bin has width fftSamplingRateHz/32, ie 41.65Hz per bin at 1333Hz -// Usable bandwidth is half this, ie 666Hz if fftSamplingRateHz is 1333Hz, i.e. bin 1 is 41.65hz, bin 2 83.3hz etc - -#define DYN_NOTCH_SMOOTH_HZ 4 -#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) // 16 -#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // 4 steps per axis +#define DYN_NOTCH_SMOOTH_HZ 4 +#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * STEP_COUNT) // 3 axes and 4 steps per axis #define DYN_NOTCH_OSD_MIN_THROTTLE 20 -static uint16_t FAST_DATA_ZERO_INIT fftSamplingRateHz; -static float FAST_DATA_ZERO_INIT fftResolution; -static uint8_t FAST_DATA_ZERO_INIT fftStartBin; -static float FAST_DATA_ZERO_INIT dynNotchQ; -static float FAST_DATA_ZERO_INIT dynNotch1Ctr; -static float FAST_DATA_ZERO_INIT dynNotch2Ctr; +typedef enum { + + STEP_WINDOW, + STEP_DETECT_PEAKS, + STEP_CALC_FREQUENCIES, + STEP_UPDATE_FILTERS, + STEP_COUNT + +} step_e; + +typedef struct peak_s { + + uint8_t bin; + float value; + +} peak_t; + +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 sdftResolutionHz; +static uint8_t FAST_DATA_ZERO_INIT sdftStartBin; +static uint8_t 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 bool FAST_DATA dualNotch = true; static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxFFT; static float FAST_DATA_ZERO_INIT smoothFactor; -static uint8_t FAST_DATA_ZERO_INIT samples; -// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window -static FAST_DATA_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE]; +static uint8_t FAST_DATA_ZERO_INIT numSamples; void gyroDataAnalyseInit(uint32_t targetLooptimeUs) { @@ -111,33 +128,29 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) gyroAnalyseInitialized = true; #endif - dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f; - dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f; - dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f; + dynNotchBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz; dynNotchMinHz = gyroConfig()->dyn_notch_min_hz; dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz); - if (gyroConfig()->dyn_notch_width_percent == 0) { - dualNotch = false; - } + // gyroDataAnalyse() is running at targetLoopRateHz (which is PID loop rate aka. 1e6f/gyro.targetLooptimeUs) + const int32_t targetLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f); + numSamples = MAX(1, targetLoopRateHz / (2 * dynNotchMaxHz)); // 600hz, 8k looptime, 6.00 - const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f); - samples = MAX(1, gyroLoopRateHz / (2 * dynNotchMaxHz)); //600hz, 8k looptime, 13.333 + sdftSampleRateHz = targetLoopRateHz / numSamples; + // eg 8k, user max 600hz, int(8000/1200) = 6 (6.666), sdftSampleRateHz = 1333hz, range 666Hz + // eg 4k, user max 600hz, int(4000/1200) = 3 (3.333), sdftSampleRateHz = 1333hz, range 666Hz + // eg 2k, user max 600hz, int(2000/1200) = 1 (1.666) sdftSampleRateHz = 2000hz, range 1000Hz + // eg 2k, user max 400hz, int(2000/800) = 2 (2.5) sdftSampleRateHz = 1000hz, range 500Hz + // 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) - fftSamplingRateHz = gyroLoopRateHz / samples; - // eg 8k, user max 600hz, int(8000/1200) = 6 (6.666), fftSamplingRateHz = 1333hz, range 666Hz - // eg 4k, user max 600hz, int(4000/1200) = 3 (3.333), fftSamplingRateHz = 1333hz, range 666Hz - // eg 2k, user max 600hz, int(2000/1200) = 1 (1.666) fftSamplingRateHz = 2000hz, range 1000Hz - // eg 2k, user max 400hz, int(2000/800) = 2 (2.5) fftSamplingRateHz = 1000hz, range 500Hz - // eg 1k, user max 600hz, int(1000/1200) = 1 (max(1,0.8333)) fftSamplingRateHz = 1000hz, range 500Hz - // the upper limit of DN is always going to be Nyquist + 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 - fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; // 41.65hz per bin for medium - fftStartBin = MAX(2, dynNotchMinHz / lrintf(fftResolution)); // can't use bin 0 because it is DC. - smoothFactor = 2 * M_PIf * DYN_NOTCH_SMOOTH_HZ / (gyroLoopRateHz / 12); // minimum PT1 k value - - for (int i = 0; i < FFT_WINDOW_SIZE; i++) { - hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples); } } @@ -145,254 +158,217 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime { // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later gyroDataAnalyseInit(targetLooptimeUs); - state->maxSampleCount = samples; + state->maxSampleCount = numSamples; state->maxSampleCountRcp = 1.0f / state->maxSampleCount; - arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE); - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - // any init value - state->centerFreq[axis] = dynNotchMaxHz; + + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (uint8_t 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; + } } } -void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample) +// Collect gyro data, to be downsampled and analysed in gyroDataAnalyse() function +void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample) { state->oversampledGyroAccumulator[axis] += sample; } -static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2); +static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state); -/* - * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function - */ -void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2) +// Downsample and analyse gyro data +FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state) { // samples should have been pushed by `gyroDataAnalysePush` // if gyro sampling is > 1kHz, accumulate and average multiple gyro samples - state->sampleCount++; - if (state->sampleCount == state->maxSampleCount) { state->sampleCount = 0; // calculate mean value of accumulated samples - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp; - state->downsampledGyroData[axis][state->circularBufferIdx] = sample; + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + const float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp; + state->downsampledGyroData[axis] = sample; if (axis == 0) { DEBUG_SET(DEBUG_FFT, 2, lrintf(sample)); } - state->oversampledGyroAccumulator[axis] = 0; } - state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE; - - // We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value + // We need DYN_NOTCH_CALC_TICKS ticks to update all axes with newly sampled value // recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls - // at 4kHz gyro loop rate this means 8kHz / 4 / 3 = 666Hz => update every 1.5ms - // at 4kHz gyro loop rate this means 4kHz / 4 / 3 = 333Hz => update every 3ms + // at 8kHz PID loop rate this means 8kHz / 4 / 3 = 666Hz => update every 1.5ms + // at 4kHz PID loop rate this means 4kHz / 4 / 3 = 333Hz => update every 3ms state->updateTicks = DYN_NOTCH_CALC_TICKS; } - // calculate FFT and update filters + // 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); + } + state->sampleCount++; + + // Find frequency peaks and update filters if (state->updateTicks > 0) { - gyroDataAnalyseUpdate(state, notchFilterDyn, notchFilterDyn2); + gyroDataAnalyseUpdate(state); --state->updateTicks; } } -void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut); -void arm_cfft_radix8by2_f32(arm_cfft_instance_f32 *S, float32_t *p1); -void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1); -void arm_radix8_butterfly_f32(float32_t *pSrc, uint16_t fftLen, const float32_t *pCoef, uint16_t twidCoefModifier); -void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable); - -/* - * Analyse gyro data - */ -static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2) +// Find frequency peaks and update filters +static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) { - enum { - STEP_ARM_CFFT_F32, - STEP_BITREVERSAL, - STEP_STAGE_RFFT_F32, - STEP_ARM_CMPLX_MAG_F32, - STEP_CALC_FREQUENCIES, - STEP_UPDATE_FILTERS, - STEP_HANNING, - STEP_COUNT - }; - - arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint); - uint32_t startTime = 0; if (debugMode == (DEBUG_FFT_TIME)) { startTime = micros(); } DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep); + switch (state->updateStep) { - case STEP_ARM_CFFT_F32: + + case STEP_WINDOW: // 6us @ F722 { - switch (FFT_BIN_COUNT) { - case 16: - // 16us - arm_cfft_radix8by2_f32(Sint, state->fftData); - break; - case 32: - // 35us - arm_cfft_radix8by4_f32(Sint, state->fftData); - break; - case 64: - // 70us - arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1); - break; + sdftWinSq(&sdft[state->updateAxis], sdftData); + + // 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 + sdftMeanSq += sdftData[bin]; // sdftData is already squared (see sdftWinSq) } + sdftMeanSq /= sdftEndBin - sdftStartBin - 1; + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); break; } - case STEP_BITREVERSAL: + case STEP_DETECT_PEAKS: // 6us @ F722 { - // 6us - arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable); - DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); - state->updateStep++; - FALLTHROUGH; - } - case STEP_STAGE_RFFT_F32: - { - // 14us - // this does not work in place => fftData AND rfftData needed - stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData); + // Get memory ready for new peak data on current axis + for (uint8_t 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++) { + // 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++) { + if (sdftData[bin] > peaks[p].value) { + for (uint8_t k = gyro.notchFilterDynCount - 1; k > p; k--) { + peaks[k] = peaks[k - 1]; + } + peaks[p].bin = bin; + peaks[p].value = sdftData[bin]; + break; + } + } + bin++; // If bin is peak, next bin can't be peak => jump it + } + } + + // 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++) { + // 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) { + peak_t temp = peaks[k]; + peaks[k] = peaks[k + 1]; + peaks[k + 1] = temp; + } + } + } + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); break; } - case STEP_ARM_CMPLX_MAG_F32: + case STEP_CALC_FREQUENCIES: // 4us @ F722 { - // 8us - arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT); - DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime); - state->updateStep++; - FALLTHROUGH; - } - case STEP_CALC_FREQUENCIES: - { - // identify max bin and max/min heights - float dataMax = 0.0f; - float dataMin = 1.0f; - uint8_t binMax = 0; - float dataMinHi = 1.0f; - for (int i = fftStartBin; i < FFT_BIN_COUNT; i++) { - if (state->fftData[i] > state->fftData[i - 1]) { // bin height increased - if (state->fftData[i] > dataMax) { - dataMax = state->fftData[i]; - binMax = i; // tallest bin so far + for (uint8_t 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; + + // 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; + } + + // 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; + } + + // get centerFreq in Hz from weighted bins + float centerFreq = dynNotchMaxHz; + float sdftMeanBin = 0; + + 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]); } } } - if (binMax == 0) { // no bin increase, hold prev max bin, dataMin = 1 dataMax = 0, ie move slow - binMax = lrintf(state->centerFreq[state->updateAxis] / fftResolution); - } else { // there was a max, find min - for (int i = binMax - 1; i > 1; i--) { // look for min below max - dataMin = state->fftData[i]; - if (state->fftData[i - 1] > state->fftData[i]) { // up step below this one - break; - } - } - for (int i = binMax + 1; i < (FFT_BIN_COUNT - 1); i++) { // // look for min above max - dataMinHi = state->fftData[i]; - if (state->fftData[i] < state->fftData[i + 1]) { // up step above this one - break; - } - } - } - dataMin = fminf(dataMin, dataMinHi); - - // accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak - float squaredData = state->fftData[binMax] * state->fftData[binMax]; - float fftSum = squaredData; - float fftWeightedSum = squaredData * binMax; - - // accumulate upper shoulder unless it would be FFT_BIN_COUNT - uint8_t shoulderBin = binMax + 1; - if (shoulderBin < FFT_BIN_COUNT) { - squaredData = state->fftData[shoulderBin] * state->fftData[shoulderBin]; - fftSum += squaredData; - fftWeightedSum += squaredData * shoulderBin; - } - - // accumulate lower shoulder unless lower shoulder would be bin 0 (DC) - if (binMax > 1) { - shoulderBin = binMax - 1; - squaredData = state->fftData[shoulderBin] * state->fftData[shoulderBin]; - fftSum += squaredData; - fftWeightedSum += squaredData * shoulderBin; - } - - // get centerFreq in Hz from weighted bins - float centerFreq = dynNotchMaxHz; - float fftMeanIndex = 0; - if (fftSum > 0) { - fftMeanIndex = (fftWeightedSum / fftSum); - centerFreq = fftMeanIndex * fftResolution; - // In theory, the index points to the centre frequency of the bin. - // at 1333hz, bin widths are 41.65Hz, so bin 2 has the range 83,3Hz to 124,95Hz - // Rav feels that maybe centerFreq = (fftMeanIndex + 0.5) * fftResolution; is better - // empirical checking shows that not adding 0.5 works better - } else { - centerFreq = state->centerFreq[state->updateAxis]; - } - centerFreq = constrainf(centerFreq, dynNotchMinHz, dynNotchMaxHz); - - // PT1 style dynamic smoothing moves rapidly towards big peaks and slowly away, up to 8x faster - float dynamicFactor = constrainf(dataMax / dataMin, 1.0f, 8.0f); - state->centerFreq[state->updateAxis] = state->centerFreq[state->updateAxis] + smoothFactor * dynamicFactor * (centerFreq - state->centerFreq[state->updateAxis]); if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) { - dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]); + for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { + dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]); + } } - if (state->updateAxis == 0) { - DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); - DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]); - DEBUG_SET(DEBUG_FFT_FREQ, 1, lrintf(dynamicFactor * 100)); - DEBUG_SET(DEBUG_DYN_LPF, 1, state->centerFreq[state->updateAxis]); + if (state->updateAxis == gyro.gyroDebugAxis) { + for (uint8_t 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])); } -// if (state->updateAxis == 1) { -// DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]); -// } + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); break; } - case STEP_UPDATE_FILTERS: + case STEP_UPDATE_FILTERS: // 7us @ F722 { - // 7us - // calculate cutoffFreq and notch Q, update notch filter - if (dualNotch) { - biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH); - biquadFilterUpdate(¬chFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH); - } else { - biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH); + for (uint8_t 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); + } } + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; - state->updateStep++; - FALLTHROUGH; - } - case STEP_HANNING: - { - // 5us - // apply hanning window to gyro samples and store result in fftData[i] to be used in step 1 and 2 and 3 - const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx; - arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx); - if (state->circularBufferIdx > 0) { - arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx); - } - - DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); } } diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h index 7be999329..9b0a7f1df 100644 --- a/src/main/flight/gyroanalyse.h +++ b/src/main/flight/gyroanalyse.h @@ -20,40 +20,30 @@ #pragma once -#include "arm_math.h" - -#include "common/filter.h" - -#define FFT_WINDOW_SIZE 32 +#define DYN_NOTCH_COUNT_MAX 5 typedef struct gyroAnalyseState_s { + // accumulator for oversampled data => no aliasing and less noise uint8_t sampleCount; uint8_t maxSampleCount; float maxSampleCountRcp; float oversampledGyroAccumulator[XYZ_AXIS_COUNT]; - // downsampled gyro data circular buffer for frequency analysis - uint8_t circularBufferIdx; - float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; + // downsampled gyro data for frequency analysis + float downsampledGyroData[XYZ_AXIS_COUNT]; // update state machine step information uint8_t updateTicks; uint8_t updateStep; uint8_t updateAxis; - arm_rfft_fast_instance_f32 fftInstance; - float fftData[FFT_WINDOW_SIZE]; - float rfftData[FFT_WINDOW_SIZE]; - - float centerFreq[XYZ_AXIS_COUNT]; + float centerFreq[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX]; } gyroAnalyseState_t; -STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type); - void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs); -void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample); -void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2); +void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t 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 352a692fd..ad9adef0f 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1770,9 +1770,9 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) #endif // Added in MSP API 1.42 #if defined(USE_GYRO_DATA_ANALYSE) - sbufWriteU8(dst, 0); // DEPRECATED 1.43: dyn_notch_range - sbufWriteU8(dst, gyroConfig()->dyn_notch_width_percent); - sbufWriteU16(dst, gyroConfig()->dyn_notch_q); + 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_min_hz); #else sbufWriteU8(dst, 0); @@ -1799,6 +1799,13 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) #else sbufWriteU8(dst, 0); #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; case MSP_PID_ADVANCED: @@ -2635,9 +2642,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, if (sbufBytesRemaining(src) >= 8) { // Added in MSP API 1.42 #if defined(USE_GYRO_DATA_ANALYSE) - sbufReadU8(src); // DEPRECATED: dyn_notch_range - gyroConfigMutable()->dyn_notch_width_percent = sbufReadU8(src); - gyroConfigMutable()->dyn_notch_q = sbufReadU16(src); + 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_min_hz = sbufReadU16(src); #else sbufReadU8(src); @@ -2661,12 +2668,19 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU16(src); #endif } - if (sbufBytesRemaining(src) >= 1) { + if (sbufBytesRemaining(src) >= 4) { // Added in MSP API 1.44 #if defined(USE_DYN_LPF) currentPidProfile->dyn_lpf_curve_expo = sbufReadU8(src); #else sbufReadU8(src); +#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 411f67b3b..93ecbab10 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -98,7 +98,7 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor1.gyroDev; #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro) #define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro) -PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8); +PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9); #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 @@ -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_width_percent = 8; - gyroConfig->dyn_notch_q = 120; + gyroConfig->dyn_notch_count = 1; + gyroConfig->dyn_notch_bandwidth_hz = 45; gyroConfig->dyn_notch_min_hz = 150; gyroConfig->gyro_filter_debug_axis = FD_ROLL; gyroConfig->dyn_lpf_curve_expo = 5; @@ -484,7 +484,7 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs) #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { - gyroDataAnalyse(&gyro.gyroAnalyseState, gyro.notchFilterDyn, gyro.notchFilterDyn2); + gyroDataAnalyse(&gyro.gyroAnalyseState); } #endif diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 52c9a59c3..b18094edf 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -107,12 +107,11 @@ typedef struct gyro_s { filterApplyFnPtr notchFilter2ApplyFn; biquadFilter_t notchFilter2[XYZ_AXIS_COUNT]; - filterApplyFnPtr notchFilterDynApplyFn; - filterApplyFnPtr notchFilterDynApplyFn2; - biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; - biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT]; - #ifdef USE_GYRO_DATA_ANALYSE + filterApplyFnPtr notchFilterDynApplyFn; + biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX]; + uint8_t notchFilterDynCount; + gyroAnalyseState_t gyroAnalyseState; #endif @@ -196,8 +195,8 @@ typedef struct gyroConfig_s { uint16_t dyn_lpf_gyro_max_hz; uint16_t dyn_notch_max_hz; - uint8_t dyn_notch_width_percent; - uint16_t dyn_notch_q; + uint8_t dyn_notch_count; + uint16_t dyn_notch_bandwidth_hz; uint16_t dyn_notch_min_hz; uint8_t gyro_filter_debug_axis; diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c index e9e9917ee..51b81cca2 100644 --- a/src/main/sensors/gyro_filter_impl.c +++ b/src/main/sensors/gyro_filter_impl.c @@ -49,16 +49,6 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void) // DEBUG_GYRO_SAMPLE(1) Record the post-downsample value for the selected debug axis GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 1, lrintf(gyroADCf)); -#ifdef USE_GYRO_DATA_ANALYSE - if (isDynamicFilterActive()) { - if (axis == gyro.gyroDebugAxis) { - GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); - GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); - GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf)); - } - } -#endif - #ifdef USE_RPM_FILTER gyroADCf = rpmFilterGyro(axis, gyroADCf); #endif @@ -76,14 +66,21 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void) #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { + if (axis == gyro.gyroDebugAxis) { + GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); + GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); + GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 0, lrintf(gyroADCf)); + } + + gyroDataAnalysePush(&gyro.gyroAnalyseState, axis, gyroADCf); + for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { + gyroADCf = gyro.notchFilterDynApplyFn((filter_t*)&gyro.notchFilterDyn[axis][p], gyroADCf); + } + if (axis == gyro.gyroDebugAxis) { GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); - GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_DYN_LPF, 3, lrintf(gyroADCf)); } - gyroDataAnalysePush(&gyro.gyroAnalyseState, axis, gyroADCf); - gyroADCf = gyro.notchFilterDynApplyFn((filter_t *)&gyro.notchFilterDyn[axis], gyroADCf); - gyroADCf = gyro.notchFilterDynApplyFn2((filter_t *)&gyro.notchFilterDyn2[axis], gyroADCf); } #endif diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 87222a36d..9fda10993 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -133,17 +133,16 @@ static void gyroInitFilterNotch2(uint16_t notchHz, uint16_t notchCutoffHz) static void gyroInitFilterDynamicNotch() { gyro.notchFilterDynApplyFn = nullFilterApply; - gyro.notchFilterDynApplyFn2 = nullFilterApply; if (isDynamicFilterActive()) { gyro.notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2 - if(gyroConfig()->dyn_notch_width_percent != 0) { - gyro.notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2 - } + gyro.notchFilterDynCount = gyroConfig()->dyn_notch_count; + const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInit(&gyro.notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); - biquadFilterInit(&gyro.notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { + biquadFilterInit(&gyro.notchFilterDyn[axis][p], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); + } } } }