From 1e960c95eb344bf4124615a635e8a5760582a073 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sun, 5 Aug 2018 17:51:28 +1000 Subject: [PATCH 01/10] Dynamic Filter Update Improves on earlier 3.5RC1 dynamic filter with: - better FFT tracking performance overall, even with a narrower default notch width - can reach 850Hz for high kV 2.5-3" and 6S quads - works better with 32k gyros - can be applied pre- (location 1) and post- (location 2) static filters. Pre-static filter location works best, but post-static may work well in 32k modes or with PT1 option - option to use a PT1 filter rather than the classical notch filter, perhaps useful for quads with a lot of noise above their peak. - ability to totally bypass the pre-FFT bandpass filter, by setting Q=0, maximising the range of responsiveness - "ignore" value, absolute FFT bin amplitude below which the bin will be excluded from consideration. May be tweaked to optimise peak detection; primarily for advanced tuners. If too high, only the very peaks will be included, if too low, noise may clutter peak detection. - "threshold" value for peak detection, which, when divided by 10, is the multiple by which one bin must exceed the previous one for peak detection to commence. If too low, FFT detection becomes more random, if too high, no peaks are detected. --- src/main/interface/settings.c | 18 +++- src/main/interface/settings.h | 4 + src/main/sensors/gyro.c | 55 +++++++++--- src/main/sensors/gyro.h | 11 ++- src/main/sensors/gyro_filter_impl.h | 21 +++-- src/main/sensors/gyroanalyse.c | 127 ++++++++++++++++++---------- src/main/sensors/gyroanalyse.h | 8 +- 7 files changed, 179 insertions(+), 65 deletions(-) diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 2f1f444cb..9155f4e0a 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -372,6 +372,12 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = { }; #endif // USE_RC_SMOOTHING_FILTER +#ifdef USE_GYRO_DATA_ANALYSE +static const char * const lookupTableDynamicFilterLocation[] = { + "BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS" +}; +#endif // USE_GYRO_DATA_ANALYSE + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -461,6 +467,10 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType), LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType), #endif // USE_RC_SMOOTHING_FILTER +#ifdef USE_GYRO_DATA_ANALYSE + LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterLocation), +#endif // USE_GYRO_DATA_ANALYSE + }; #undef LOOKUP_TABLE_ENTRY @@ -506,8 +516,12 @@ const clivalue_t valueTable[] = { { "gyro_to_use", VAR_UINT8 | MASTER_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_quality", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) }, - { "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) }, + { "dyn_filter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_type) }, + { "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) }, + { "dyn_notch_quality", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) }, + { "dyn_filter_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_location) }, + { "dyn_filter_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_threshold) }, + { "dyn_filter_ignore", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_ignore) }, #endif // PG_ACCELEROMETER_CONFIG diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index 6dacdcb33..8acd70bd9 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -112,6 +112,10 @@ typedef enum { TABLE_RC_SMOOTHING_INPUT_TYPE, TABLE_RC_SMOOTHING_DERIVATIVE_TYPE, #endif // USE_RC_SMOOTHING_FILTER +#ifdef USE_GYRO_DATA_ANALYSE + TABLE_DYNAMIC_FILTER_LOCATION, +#endif // USE_GYRO_DATA_ANALYSE + LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c48bf996d..a4a2971df 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -132,8 +132,8 @@ typedef struct gyroSensor_s { filterApplyFnPtr notchFilter2ApplyFn; biquadFilter_t notchFilter2[XYZ_AXIS_COUNT]; - filterApplyFnPtr notchFilterDynApplyFn; - biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; + filterApplyFnPtr dynFilterApplyFn; + gyroDynamicFilter_t dynFilter[XYZ_AXIS_COUNT]; // overflow and recovery timeUs_t overflowTimeUs; @@ -144,8 +144,12 @@ typedef struct gyroSensor_s { #endif // USE_YAW_SPIN_RECOVERY #ifdef USE_GYRO_DATA_ANALYSE +#define DYNAMIC_LOWPASS_DEFAULT_CUTOFF_HZ 200 +#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 400 +#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 350 gyroAnalyseState_t gyroAnalyseState; #endif + } gyroSensor_t; STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1; @@ -203,8 +207,12 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_offset_yaw = 0, .yaw_spin_recovery = true, .yaw_spin_threshold = 1950, - .dyn_notch_quality = 70, - .dyn_notch_width_percent = 50, + .dyn_filter_type = FILTER_BIQUAD, + .dyn_filter_width_percent = 40, + .dyn_notch_quality = 20, + .dyn_filter_location = DYN_FILTER_BEFORE_STATIC_FILTERS, + .dyn_filter_threshold = 30, + .dyn_filter_ignore = 20, ); @@ -510,6 +518,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) #ifdef USE_GYRO_DATA_ANALYSE gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime); + #endif return true; @@ -662,7 +671,7 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint // Dereference the pointer to null before checking valid cutoff and filter // type. It will be overridden for positive cases. - *lowpassFilterApplyFn = &nullFilterApply; + *lowpassFilterApplyFn = nullFilterApply; // If lowpass cutoff has been specified and is less than the Nyquist frequency if (lpfHz && lpfHz <= gyroFrequencyNyquist) { @@ -742,15 +751,35 @@ static bool isDynamicFilterActive(void) return feature(FEATURE_DYNAMIC_FILTER); } -static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) +static void gyroInitFilterDynamic(gyroSensor_t *gyroSensor) { - gyroSensor->notchFilterDynApplyFn = nullFilterApply; + gyroSensor->dynFilterApplyFn = nullFilterApply; if (isDynamicFilterActive()) { - gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2 - const float notchQ = filterGetNotchQ(400, 390); //just any init value - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH); + switch (gyroConfig()->dyn_filter_type) { + case FILTER_PT1: { + const float gyroDt = gyro.targetLooptime * 1e-6f; + const float gain = pt1FilterGain(DYNAMIC_LOWPASS_DEFAULT_CUTOFF_HZ, gyroDt); + + gyroSensor->dynFilterApplyFn = (filterApplyFnPtr) pt1FilterApply; + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt1FilterInit(&gyroSensor->dynFilter[axis].pt1FilterState, gain); + } + + break; + } + case FILTER_BIQUAD: { + const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); + + gyroSensor->dynFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1; + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + biquadFilterInit(&gyroSensor->dynFilter[axis].biquadFilterState, DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); + } + + break; + } } } } @@ -780,7 +809,7 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); #ifdef USE_GYRO_DATA_ANALYSE - gyroInitFilterDynamicNotch(gyroSensor); + gyroInitFilterDynamic(gyroSensor); #endif } @@ -1075,7 +1104,7 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { - gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn); + gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->dynFilter); } #endif } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b72662034..e6d57d047 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -58,6 +58,11 @@ typedef enum { GYRO_OVERFLOW_CHECK_ALL_AXES } gyroOverflowCheck_e; +enum { + DYN_FILTER_BEFORE_STATIC_FILTERS = 0, + DYN_FILTER_AFTER_STATIC_FILTERS +} ; + #define GYRO_CONFIG_USE_GYRO_1 0 #define GYRO_CONFIG_USE_GYRO_2 1 #define GYRO_CONFIG_USE_GYRO_BOTH 2 @@ -96,8 +101,12 @@ typedef struct gyroConfig_s { int16_t yaw_spin_threshold; uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second + uint8_t dyn_filter_type; + uint8_t dyn_filter_width_percent; uint8_t dyn_notch_quality; // bandpass quality factor, 100 for steep sided bandpass - uint8_t dyn_notch_width_percent; + uint8_t dyn_filter_location; // before or after static filters + uint8_t dyn_filter_threshold; // divided by 10 then difference needed to detect peak + uint8_t dyn_filter_ignore; // ignore any FFT bin below this threshold } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index cd7430a5c..e79ae2f40 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -11,7 +11,15 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe if (isDynamicFilterActive()) { if (axis == X) { GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data - GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data + GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data in 3 + } + if (gyroConfig()->dyn_filter_location == DYN_FILTER_BEFORE_STATIC_FILTERS) { + gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf); + gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf); + if (axis == X) { + GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch + GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); // store post-notch data in 2 + } } } #endif @@ -24,10 +32,13 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { - gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf); - gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); - if (axis == X) { - GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch + if (gyroConfig()->dyn_filter_location == DYN_FILTER_AFTER_STATIC_FILTERS){ + if (axis == X) { + GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); // store post-static data in debug 2 + GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after statics + } + gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf); + gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf); } } #endif diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index c96e16020..50152a70a 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -45,19 +45,26 @@ // A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each with a width 31.25Hz // Eg [0,31), [31,62), [62, 93) etc -#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) -// for gyro loop >= 4KHz, analyse up to 666Hz, 16 bins each 41.625 Hz wide -#define FFT_SAMPLING_RATE_HZ 1333 +// for gyro loop >= 4KHz, analyse up to 1000Hz, 16 bins each 62.5 Hz wide +#define FFT_SAMPLING_RATE_HZ 2000 +#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE) // following bin must be at least 2 times previous to indicate start of peak -#define FFT_MIN_BIN_RISE 2 +#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) // the desired approimate lower frequency when calculating bin offset #define FFT_BIN_OFFSET_DESIRED_HZ 90 -// lowpass frequency for smoothing notch centre point -#define DYN_NOTCH_SMOOTH_FREQ_HZ 60 +// centre frequency of bandpass that constrains input to FFT +#if FFT_SAMPLING_RATE_HZ == 2000 +#define FFT_BPF_HZ 350 +#elif FFT_SAMPLING_RATE_HZ < 2000 +#define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4) +#endif +#define DYN_NOTCH_SMOOTH_FREQ_HZ 50 // notch centre point will not go below this, must be greater than cutoff, mid of bottom bin -#define DYN_NOTCH_MIN_CENTRE_HZ 125 +#define DYN_NOTCH_MIN_CENTRE_HZ 140 +// maximum notch centre frequency limited by Nyquist +#define DYN_NOTCH_MAX_CENTRE_HZ (FFT_SAMPLING_RATE_HZ / 2) // lowest allowed notch cutoff frequency -#define DYN_NOTCH_MIN_CUTOFF_HZ 105 +#define DYN_NOTCH_MIN_CUTOFF_HZ 110 // we need 4 steps for each axis #define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) @@ -66,13 +73,14 @@ static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz; static uint16_t FAST_RAM_ZERO_INIT fftBpfHz; // Hz per bin static float FAST_RAM_ZERO_INIT fftResolution; -// maximum notch centre frequency limited by Nyquist -static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCentreHz; static uint8_t FAST_RAM_ZERO_INIT fftBinOffset; // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE]; -static FAST_RAM_ZERO_INIT float dynamicNotchCutoff; +static FAST_RAM_ZERO_INIT float dynamicFilterCutoffFactor; +static FAST_RAM_ZERO_INIT uint8_t dynamicFilterType; +static FAST_RAM_ZERO_INIT float dynamicFilterThreshold; +static FAST_RAM_ZERO_INIT float dynamicFilterIgnore; void gyroDataAnalyseInit(uint32_t targetLooptimeUs) { @@ -92,7 +100,6 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) fftBpfHz = fftSamplingRateHz / 4; fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; - dynNotchMaxCentreHz = fftSamplingRateHz / 2; // Calculate the FFT bin offset to try and get the lowest bin used // in the center calc close to 90hz @@ -103,7 +110,10 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); } - dynamicNotchCutoff = (100.0f - gyroConfig()->dyn_notch_width_percent) / 100; + dynamicFilterCutoffFactor = (100.0f - gyroConfig()->dyn_filter_width_percent) / 100; + dynamicFilterType = gyroConfig()->dyn_filter_type; + dynamicFilterThreshold = gyroConfig()->dyn_filter_threshold / 10; + dynamicFilterIgnore = gyroConfig()->dyn_filter_ignore / 10; } void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) @@ -123,7 +133,8 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS); for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // any init value - state->centerFreq[axis] = 200; + state->centerFreq[axis] = DYN_NOTCH_MAX_CENTRE_HZ; + state->prevCenterFreq[axis] = DYN_NOTCH_MAX_CENTRE_HZ; biquadFilterInit(&state->gyroBandpassFilter[axis], fftBpfHz, 1000000 / fftSamplingRateHz, 0.01f * gyroConfig()->dyn_notch_quality, FILTER_BPF); biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); } @@ -134,12 +145,12 @@ void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float state->oversampledGyroAccumulator[axis] += sample; } -static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn); +static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter); /* * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function */ -void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) +void gyroDataAnalyse(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter) { // samples should have been pushed by `gyroDataAnalysePush` // if gyro sampling is > 1kHz, accumulate multiple samples @@ -152,8 +163,9 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) // calculate mean value of accumulated samples for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp; - sample = biquadFilterApply(&state->gyroBandpassFilter[axis], sample); - + if (gyroConfig()->dyn_notch_quality > 4){ + sample = biquadFilterApply(&state->gyroBandpassFilter[axis], sample); + } state->downsampledGyroData[axis][state->circularBufferIdx] = sample; if (axis == 0) { DEBUG_SET(DEBUG_FFT, 2, lrintf(sample)); @@ -170,7 +182,7 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) // calculate FFT and update filters if (state->updateTicks > 0) { - gyroDataAnalyseUpdate(state, notchFilterDyn); + gyroDataAnalyseUpdate(state, dynFilter); --state->updateTicks; } } @@ -184,7 +196,7 @@ void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t /* * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds */ -static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) +static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter) { enum { STEP_ARM_CFFT_F32, @@ -255,59 +267,88 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, // calculate FFT centreFreq float fftSum = 0; float fftWeightedSum = 0; + float dataAvg = 0; bool fftIncreasing = false; + //get simple average of bin amplitudes + for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { + dataAvg += state->fftData[i]; + } + dataAvg = dataAvg / FFT_BIN_COUNT; + // iterate over fft data and calculate weighted indices for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { const float data = state->fftData[i]; const float prevData = state->fftData[i - 1]; - - if (fftIncreasing || data > prevData * FFT_MIN_BIN_RISE) { - float cubedData = data * data * data; - - // add previous bin before first rise - if (!fftIncreasing) { - cubedData += prevData * prevData * prevData; - - fftIncreasing = true; + // only consider bins above ignore multiple of average size + if (data > dynamicFilterIgnore * dataAvg) { + // only consider bins after > threshold step up from previous + if (fftIncreasing || data > prevData * dynamicFilterThreshold) { + float cubedData = data * data * data; + // add previous bin before first rise + if (!fftIncreasing) { + cubedData += prevData * prevData * prevData; + fftIncreasing = true; + } + fftSum += cubedData; + // calculate weighted index starting at 1, not 0 + fftWeightedSum += cubedData * (i + 1); } - - fftSum += cubedData; - // calculate weighted index starting at 1, not 0 - fftWeightedSum += cubedData * (i + 1); } } // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz) // if no peak, go to highest point to minimise delay - float centerFreq = dynNotchMaxCentreHz; + float centerFreq = DYN_NOTCH_MAX_CENTRE_HZ; float fftMeanIndex = 0; - if (fftSum > 0) { // idx was shifted by 1 to start at 1, not 0 fftMeanIndex = (fftWeightedSum / fftSum) - 1; // the index points at the center frequency of each bin so index 0 is actually 16.125Hz - centerFreq = constrain(fftMeanIndex * fftResolution, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz); + centerFreq = fftMeanIndex * FFT_RESOLUTION; + } else { + centerFreq = state->prevCenterFreq[state->updateAxis]; } - + state->prevCenterFreq[state->updateAxis] = centerFreq; + centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ); centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq); - centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz); + centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ); state->centerFreq[state->updateAxis] = centerFreq; if (state->updateAxis == 0) { DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); } - DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis, state->centerFreq[state->updateAxis]); + + if (state->updateAxis == 0 || state->updateAxis == 1) { + DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis, state->centerFreq[state->updateAxis]); + } DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); + break; } case STEP_UPDATE_FILTERS: { // 7us - // calculate cutoffFreq and notch Q, update notch filter - const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicNotchCutoff, DYN_NOTCH_MIN_CUTOFF_HZ); - const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq); - biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH); + switch (dynamicFilterType) { + case FILTER_PT1: { + const int cutoffFreq = state->centerFreq[state->updateAxis] * dynamicFilterCutoffFactor; + const float gyroDt = gyro.targetLooptime * 1e-6f; + const float gain = pt1FilterGain(cutoffFreq, gyroDt); + + pt1FilterUpdateCutoff(&dynFilter[state->updateAxis].pt1FilterState, gain); + + break; + } + case FILTER_BIQUAD: { + // calculate cutoffFreq and notch Q, update notch filter + const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterCutoffFactor, DYN_NOTCH_MIN_CUTOFF_HZ); + const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq); + biquadFilterUpdate(&dynFilter[state->updateAxis].biquadFilterState, state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH); + + break; + } + } + DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index 259342c7b..4a8de2643 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -28,6 +28,11 @@ // max for F3 targets #define FFT_WINDOW_SIZE 32 +typedef union gyroDynamicFilter_u { + pt1Filter_t pt1FilterState; + biquadFilter_t biquadFilterState; +} gyroDynamicFilter_t; + typedef struct gyroAnalyseState_s { // accumulator for oversampled data => no aliasing and less noise uint8_t sampleCount; @@ -53,10 +58,11 @@ typedef struct gyroAnalyseState_s { biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT]; uint16_t centerFreq[XYZ_AXIS_COUNT]; + uint16_t prevCenterFreq[XYZ_AXIS_COUNT]; } gyroAnalyseState_t; STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type); void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime); void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample); -void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn); +void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, gyroDynamicFilter_t *dynFilter); From 3f001295f7a83d251147d80cb8fb38346c937a2a Mon Sep 17 00:00:00 2001 From: Andrey Mironov Date: Wed, 8 Aug 2018 11:24:16 +0300 Subject: [PATCH 02/10] Simplified FFT location --- src/main/interface/settings.c | 8 ++++---- src/main/interface/settings.h | 2 +- src/main/sensors/gyro.c | 2 +- src/main/sensors/gyro.h | 6 +++--- src/main/sensors/gyro_filter_impl.h | 31 +++++++++++++---------------- 5 files changed, 23 insertions(+), 26 deletions(-) diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 9155f4e0a..506d01860 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -373,7 +373,7 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = { #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_GYRO_DATA_ANALYSE -static const char * const lookupTableDynamicFilterLocation[] = { +static const char * const lookupTableDynamicFftLocation[] = { "BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS" }; #endif // USE_GYRO_DATA_ANALYSE @@ -468,7 +468,7 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType), #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_GYRO_DATA_ANALYSE - LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterLocation), + LOOKUP_TABLE_ENTRY(lookupTableDynamicFftLocation), #endif // USE_GYRO_DATA_ANALYSE }; @@ -516,12 +516,12 @@ const clivalue_t valueTable[] = { { "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, #endif #if defined(USE_GYRO_DATA_ANALYSE) + { "dyn_fft_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FFT_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_fft_location) }, { "dyn_filter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_type) }, { "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) }, - { "dyn_notch_quality", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) }, - { "dyn_filter_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_location) }, { "dyn_filter_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_threshold) }, { "dyn_filter_ignore", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_ignore) }, + { "dyn_notch_quality", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) }, #endif // PG_ACCELEROMETER_CONFIG diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index 8acd70bd9..f65435ac3 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -113,7 +113,7 @@ typedef enum { TABLE_RC_SMOOTHING_DERIVATIVE_TYPE, #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_GYRO_DATA_ANALYSE - TABLE_DYNAMIC_FILTER_LOCATION, + TABLE_DYNAMIC_FFT_LOCATION, #endif // USE_GYRO_DATA_ANALYSE LOOKUP_TABLE_COUNT diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a4a2971df..e44256e7e 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -210,7 +210,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .dyn_filter_type = FILTER_BIQUAD, .dyn_filter_width_percent = 40, .dyn_notch_quality = 20, - .dyn_filter_location = DYN_FILTER_BEFORE_STATIC_FILTERS, + .dyn_fft_location = DYN_FFT_BEFORE_STATIC_FILTERS, .dyn_filter_threshold = 30, .dyn_filter_ignore = 20, ); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index e6d57d047..9f12dac1b 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -59,8 +59,8 @@ typedef enum { } gyroOverflowCheck_e; enum { - DYN_FILTER_BEFORE_STATIC_FILTERS = 0, - DYN_FILTER_AFTER_STATIC_FILTERS + DYN_FFT_BEFORE_STATIC_FILTERS = 0, + DYN_FFT_AFTER_STATIC_FILTERS } ; #define GYRO_CONFIG_USE_GYRO_1 0 @@ -104,7 +104,7 @@ typedef struct gyroConfig_s { uint8_t dyn_filter_type; uint8_t dyn_filter_width_percent; uint8_t dyn_notch_quality; // bandpass quality factor, 100 for steep sided bandpass - uint8_t dyn_filter_location; // before or after static filters + uint8_t dyn_fft_location; // before or after static filters uint8_t dyn_filter_threshold; // divided by 10 then difference needed to detect peak uint8_t dyn_filter_ignore; // ignore any FFT bin below this threshold } gyroConfig_t; diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index e79ae2f40..fdbc8cd26 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -10,18 +10,12 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { if (axis == X) { - GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data - GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data in 3 - } - if (gyroConfig()->dyn_filter_location == DYN_FILTER_BEFORE_STATIC_FILTERS) { - gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf); - gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf); - if (axis == X) { - GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch - GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); // store post-notch data in 2 - } + GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); + GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); } } + + float gyroDataForAnalysis = gyroADCf; #endif // apply static notch filters and software lowpass filters @@ -32,14 +26,17 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { - if (gyroConfig()->dyn_filter_location == DYN_FILTER_AFTER_STATIC_FILTERS){ - if (axis == X) { - GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); // store post-static data in debug 2 - GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after statics - } - gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf); - gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf); + if (gyroConfig()->dyn_fft_location == DYN_FFT_AFTER_STATIC_FILTERS) { + gyroDataForAnalysis = gyroADCf; } + + if (axis == X) { + GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroDataForAnalysis)); + GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis)); + } + + gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroDataForAnalysis); + gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf); } #endif From 14c90bf10b865a66b3f99df0cd2cbe4448d2e8c3 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Mon, 13 Aug 2018 15:00:24 +1000 Subject: [PATCH 03/10] user configurable sample rate, true peak detection 1. User can set sampling rate to suit expected range of frequencies: - HIGH suits 4" or smaller and 6S 5" - MEDIUM suits classic 5" 4S - LOW is for 6" or greater Limits automatically scaled: HIGH : 133/166 to 1000Hz, MEDIUM : 89/111 to 666Hz, LOW : 67/83 to 500Hz 2. Bandpass entirely eliminated, not needed. 3. True peak detection method, favouring first peak to exceed 80% of maximum bin height; ignore or threshold values not required. --- src/main/interface/settings.c | 8 +- src/main/interface/settings.h | 1 + src/main/sensors/gyro.c | 6 +- src/main/sensors/gyro.h | 11 ++- src/main/sensors/gyroanalyse.c | 150 +++++++++++++++++---------------- 5 files changed, 93 insertions(+), 83 deletions(-) diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 506d01860..7b749a677 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -376,6 +376,9 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = { static const char * const lookupTableDynamicFftLocation[] = { "BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS" }; +static const char * const lookupTableDynamicFilterRange[] = { + "HIGH", "MEDIUM", "LOW" +}; #endif // USE_GYRO_DATA_ANALYSE #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } @@ -469,6 +472,7 @@ const lookupTableEntry_t lookupTables[] = { #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_GYRO_DATA_ANALYSE LOOKUP_TABLE_ENTRY(lookupTableDynamicFftLocation), + LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterRange), #endif // USE_GYRO_DATA_ANALYSE }; @@ -519,9 +523,7 @@ const clivalue_t valueTable[] = { { "dyn_fft_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FFT_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_fft_location) }, { "dyn_filter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_type) }, { "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) }, - { "dyn_filter_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_threshold) }, - { "dyn_filter_ignore", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_ignore) }, - { "dyn_notch_quality", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) }, + { "dyn_filter_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) }, #endif // PG_ACCELEROMETER_CONFIG diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index f65435ac3..767755445 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -114,6 +114,7 @@ typedef enum { #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_GYRO_DATA_ANALYSE TABLE_DYNAMIC_FFT_LOCATION, + TABLE_DYNAMIC_FILTER_RANGE, #endif // USE_GYRO_DATA_ANALYSE LOOKUP_TABLE_COUNT diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index e44256e7e..cb412bd0d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -209,10 +209,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .yaw_spin_threshold = 1950, .dyn_filter_type = FILTER_BIQUAD, .dyn_filter_width_percent = 40, - .dyn_notch_quality = 20, - .dyn_fft_location = DYN_FFT_BEFORE_STATIC_FILTERS, - .dyn_filter_threshold = 30, - .dyn_filter_ignore = 20, + .dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS, + .dyn_filter_range = DYN_FILTER_RANGE_MEDIUM, ); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 9f12dac1b..8b711cfa4 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -63,6 +63,12 @@ enum { DYN_FFT_AFTER_STATIC_FILTERS } ; +enum { + DYN_FILTER_RANGE_HIGH = 0, + DYN_FILTER_RANGE_MEDIUM, + DYN_FILTER_RANGE_LOW +} ; + #define GYRO_CONFIG_USE_GYRO_1 0 #define GYRO_CONFIG_USE_GYRO_2 1 #define GYRO_CONFIG_USE_GYRO_BOTH 2 @@ -101,12 +107,11 @@ typedef struct gyroConfig_s { int16_t yaw_spin_threshold; uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second + uint8_t dyn_filter_type; uint8_t dyn_filter_width_percent; - uint8_t dyn_notch_quality; // bandpass quality factor, 100 for steep sided bandpass uint8_t dyn_fft_location; // before or after static filters - uint8_t dyn_filter_threshold; // divided by 10 then difference needed to detect peak - uint8_t dyn_filter_ignore; // ignore any FFT bin below this threshold + uint8_t dyn_filter_range; // ignore any FFT bin below this threshold } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 50152a70a..2460fb946 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -42,45 +42,36 @@ #include "sensors/gyroanalyse.h" // The FFT splits the frequency domain into an number of bins -// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each with a width 31.25Hz +// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each 31.25Hz wide // Eg [0,31), [31,62), [62, 93) etc -// for gyro loop >= 4KHz, analyse up to 1000Hz, 16 bins each 62.5 Hz wide -#define FFT_SAMPLING_RATE_HZ 2000 -#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE) -// following bin must be at least 2 times previous to indicate start of peak +// for gyro loop >= 4KHz, sample rate 2000 defines to 1000Hz, 16 bins each 62.5 Hz wide +// NB FFT_WINDOW_SIZE is defined as 32 in gyroanalyse.h #define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) -// the desired approimate lower frequency when calculating bin offset -#define FFT_BIN_OFFSET_DESIRED_HZ 90 -// centre frequency of bandpass that constrains input to FFT -#if FFT_SAMPLING_RATE_HZ == 2000 -#define FFT_BPF_HZ 350 -#elif FFT_SAMPLING_RATE_HZ < 2000 -#define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4) -#endif +// start to compare 3rd to 2nd bin, ie start comparing from 77Hz, 100Hz, and 150Hz centres +#define FFT_BIN_OFFSET 2 #define DYN_NOTCH_SMOOTH_FREQ_HZ 50 -// notch centre point will not go below this, must be greater than cutoff, mid of bottom bin -#define DYN_NOTCH_MIN_CENTRE_HZ 140 -// maximum notch centre frequency limited by Nyquist -#define DYN_NOTCH_MAX_CENTRE_HZ (FFT_SAMPLING_RATE_HZ / 2) -// lowest allowed notch cutoff frequency -#define DYN_NOTCH_MIN_CUTOFF_HZ 110 +// notch centre point will not go below sample rate divided by these dividers, resulting in range limits: +// HIGH : 133/166-1000Hz, MEDIUM -> 89/111-666Hz, LOW -> 67/83-500Hz +#define DYN_NOTCH_MIN_CENTRE_DIV 12 +// lowest allowed notch cutoff frequency 20% below minimum allowed notch +#define DYN_NOTCH_MIN_CUTOFF_DIV 15 // we need 4 steps for each axis #define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) -static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz; -// centre frequency of bandpass that constrains input to FFT -static uint16_t FAST_RAM_ZERO_INIT fftBpfHz; -// Hz per bin -static float FAST_RAM_ZERO_INIT fftResolution; -static uint8_t FAST_RAM_ZERO_INIT fftBinOffset; +static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz; +static float FAST_RAM_ZERO_INIT fftResolution; +static uint8_t FAST_RAM_ZERO_INIT fftBinOffset; +static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCenterHz; +static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMaxCenterHz; +static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCutoffHz; +static float FAST_RAM_ZERO_INIT dynamicFilterWidthFactor; +static uint8_t FAST_RAM_ZERO_INIT dynamicFilterType; + +static uint8_t dynamicFilterRange; // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE]; -static FAST_RAM_ZERO_INIT float dynamicFilterCutoffFactor; -static FAST_RAM_ZERO_INIT uint8_t dynamicFilterType; -static FAST_RAM_ZERO_INIT float dynamicFilterThreshold; -static FAST_RAM_ZERO_INIT float dynamicFilterIgnore; void gyroDataAnalyseInit(uint32_t targetLooptimeUs) { @@ -92,33 +83,40 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) gyroAnalyseInitialized = true; #endif - const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f); - + dynamicFilterType = gyroConfig()->dyn_filter_type; + dynamicFilterRange = gyroConfig()->dyn_filter_range; + + fftSamplingRateHz = 1000; + if (dynamicFilterRange == DYN_FILTER_RANGE_HIGH) { + fftSamplingRateHz = 2000; + } + else if (dynamicFilterRange == DYN_FILTER_RANGE_MEDIUM) { + fftSamplingRateHz = 1333; + } // If we get at least 3 samples then use the default FFT sample frequency // otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K) - fftSamplingRateHz = MIN((gyroLoopRateHz / 3), FFT_SAMPLING_RATE_HZ); + const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f); + + fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz); - fftBpfHz = fftSamplingRateHz / 4; fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; + fftBinOffset = FFT_BIN_OFFSET; + + dynamicNotchMaxCenterHz = fftSamplingRateHz / 2; //Nyquist + dynamicNotchMinCenterHz = fftSamplingRateHz / DYN_NOTCH_MIN_CENTRE_DIV; + dynamicNotchMinCutoffHz = fftSamplingRateHz / DYN_NOTCH_MIN_CUTOFF_DIV; + dynamicFilterWidthFactor = (100.0f - gyroConfig()->dyn_filter_width_percent) / 100; - // Calculate the FFT bin offset to try and get the lowest bin used - // in the center calc close to 90hz - // > 1333hz = 1, 889hz (2.67K) = 2, 666hz (2K) = 3 - fftBinOffset = MAX(1, lrintf(FFT_BIN_OFFSET_DESIRED_HZ / fftResolution - 1.5f)); 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))); } - - dynamicFilterCutoffFactor = (100.0f - gyroConfig()->dyn_filter_width_percent) / 100; - dynamicFilterType = gyroConfig()->dyn_filter_type; - dynamicFilterThreshold = gyroConfig()->dyn_filter_threshold / 10; - dynamicFilterIgnore = gyroConfig()->dyn_filter_ignore / 10; } void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) { // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later + // *** can this next line be removed ??? *** gyroDataAnalyseInit(targetLooptimeUs); const uint16_t samplingFrequency = 1000000 / targetLooptimeUs; @@ -127,15 +125,14 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE); - // 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 4khz / 4 / 3 = 333Hz => update every 3ms - // for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms +// 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 4khz / 4 / 3 = 333Hz => update every 3ms +// for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS); for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // any init value - state->centerFreq[axis] = DYN_NOTCH_MAX_CENTRE_HZ; - state->prevCenterFreq[axis] = DYN_NOTCH_MAX_CENTRE_HZ; - biquadFilterInit(&state->gyroBandpassFilter[axis], fftBpfHz, 1000000 / fftSamplingRateHz, 0.01f * gyroConfig()->dyn_notch_quality, FILTER_BPF); + state->centerFreq[axis] = dynamicNotchMaxCenterHz; + state->prevCenterFreq[axis] = dynamicNotchMaxCenterHz; biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); } } @@ -163,9 +160,6 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter) // calculate mean value of accumulated samples for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp; - if (gyroConfig()->dyn_notch_quality > 4){ - sample = biquadFilterApply(&state->gyroBandpassFilter[axis], sample); - } state->downsampledGyroData[axis][state->circularBufferIdx] = sample; if (axis == 0) { DEBUG_SET(DEBUG_FFT, 2, lrintf(sample)); @@ -268,28 +262,41 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, float fftSum = 0; float fftWeightedSum = 0; float dataAvg = 0; + float dataMax = 0; + float dynFiltThreshold = 0; bool fftIncreasing = false; + bool fftPeakFinished = false; - //get simple average of bin amplitudes + //get simple average and max of bin amplitudes for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { dataAvg += state->fftData[i]; + if (state->fftData[i] > dataMax) { + dataMax = state->fftData[i]; + } } + // lower Max value to catch first peak close to max + dataMax = 0.8f * dataMax; dataAvg = dataAvg / FFT_BIN_COUNT; - + // automatically set peak detection threshold at half difference between peak and average + dynFiltThreshold = 0.5f * (dataMax / dataAvg); // iterate over fft data and calculate weighted indices for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { const float data = state->fftData[i]; const float prevData = state->fftData[i - 1]; - // only consider bins above ignore multiple of average size - if (data > dynamicFilterIgnore * dataAvg) { - // only consider bins after > threshold step up from previous - if (fftIncreasing || data > prevData * dynamicFilterThreshold) { + // disregard fft bins after first peak + if (!fftPeakFinished) { + // include bins around the first bin that exceeds 80% max bin height and increased compared to previous bin + if (fftIncreasing || ((data > prevData * dynFiltThreshold) && (data > dataMax))) { float cubedData = data * data * data; - // add previous bin before first rise + // add previous bin if (!fftIncreasing) { cubedData += prevData * prevData * prevData; fftIncreasing = true; } + // peak over when incoming bin falls below average + if (data < dataAvg) { + fftPeakFinished = true; + } fftSum += cubedData; // calculate weighted index starting at 1, not 0 fftWeightedSum += cubedData * (i + 1); @@ -299,28 +306,28 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz) // if no peak, go to highest point to minimise delay - float centerFreq = DYN_NOTCH_MAX_CENTRE_HZ; + float centerFreq = dynamicNotchMaxCenterHz; float fftMeanIndex = 0; if (fftSum > 0) { // idx was shifted by 1 to start at 1, not 0 fftMeanIndex = (fftWeightedSum / fftSum) - 1; // the index points at the center frequency of each bin so index 0 is actually 16.125Hz - centerFreq = fftMeanIndex * FFT_RESOLUTION; + centerFreq = fftMeanIndex * fftResolution; } else { centerFreq = state->prevCenterFreq[state->updateAxis]; } state->prevCenterFreq[state->updateAxis] = centerFreq; - centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ); + centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz); centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq); - centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ); + centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz); state->centerFreq[state->updateAxis] = centerFreq; if (state->updateAxis == 0) { DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); + DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]); } - - if (state->updateAxis == 0 || state->updateAxis == 1) { - DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis, state->centerFreq[state->updateAxis]); + if (state->updateAxis == 1) { + DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]); } DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); @@ -331,22 +338,19 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, // 7us switch (dynamicFilterType) { case FILTER_PT1: { - const int cutoffFreq = state->centerFreq[state->updateAxis] * dynamicFilterCutoffFactor; + const int cutoffFreq = state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor; const float gyroDt = gyro.targetLooptime * 1e-6f; const float gain = pt1FilterGain(cutoffFreq, gyroDt); - pt1FilterUpdateCutoff(&dynFilter[state->updateAxis].pt1FilterState, gain); - break; - } + } case FILTER_BIQUAD: { // calculate cutoffFreq and notch Q, update notch filter - const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterCutoffFactor, DYN_NOTCH_MIN_CUTOFF_HZ); + const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor, dynamicNotchMinCutoffHz); const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq); biquadFilterUpdate(&dynFilter[state->updateAxis].biquadFilterState, state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH); - - break; - } + break; + } } DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); From c6a2ba18ef83de1244dc93760b7e62db5a2ac819 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Wed, 15 Aug 2018 17:20:12 +1000 Subject: [PATCH 04/10] Improve peak detection --- src/main/sensors/gyroanalyse.c | 44 +++++++++++++++++++--------------- 1 file changed, 25 insertions(+), 19 deletions(-) diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 2460fb946..ab89dd170 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -263,40 +263,46 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, float fftWeightedSum = 0; float dataAvg = 0; float dataMax = 0; - float dynFiltThreshold = 0; - bool fftIncreasing = false; + float dataThreshold; + bool fftPeakDetected = false; bool fftPeakFinished = false; - //get simple average and max of bin amplitudes + //get average and max of bin amplitudes once they start increasing for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { - dataAvg += state->fftData[i]; - if (state->fftData[i] > dataMax) { - dataMax = state->fftData[i]; + if (fftPeakDetected || (state->fftData[i] > state->fftData[i - 1])) { + dataAvg += state->fftData[i]; + fftPeakDetected = true; + if (state->fftData[i] > dataMax) { + dataMax = state->fftData[i]; + } } } - // lower Max value to catch first peak close to max - dataMax = 0.8f * dataMax; dataAvg = dataAvg / FFT_BIN_COUNT; - // automatically set peak detection threshold at half difference between peak and average - dynFiltThreshold = 0.5f * (dataMax / dataAvg); + + //peak, once increasing, must be more than 80% above average and 1.4 times average + dataThreshold = MAX(1.4f * dataAvg, (0.8f * dataMax + 0.2f * dataAvg)); // iterate over fft data and calculate weighted indices + fftPeakDetected = false; for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { const float data = state->fftData[i]; - const float prevData = state->fftData[i - 1]; - // disregard fft bins after first peak + const float dataPrev = state->fftData[i - 1]; + // include bins only after first peak detected if (!fftPeakFinished) { - // include bins around the first bin that exceeds 80% max bin height and increased compared to previous bin - if (fftIncreasing || ((data > prevData * dynFiltThreshold) && (data > dataMax))) { + // peak must exceed thresholds and come after an increase in bin height + if (fftPeakDetected || (data > dataPrev && data > dataThreshold)) { + // add current bin float cubedData = data * data * data; - // add previous bin - if (!fftIncreasing) { - cubedData += prevData * prevData * prevData; - fftIncreasing = true; + // indicate peak detected + if (!fftPeakDetected) { + fftPeakDetected = true; + // accumulate previous bin + cubedData += dataPrev * dataPrev * dataPrev; } - // peak over when incoming bin falls below average + // terminate when peak ends ie data falls below average if (data < dataAvg) { fftPeakFinished = true; } + //calculate sums fftSum += cubedData; // calculate weighted index starting at 1, not 0 fftWeightedSum += cubedData * (i + 1); From 2bc6f92c1bd3bc0ec81640ec46432dd1638c272c Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Wed, 15 Aug 2018 19:13:00 +1000 Subject: [PATCH 05/10] move definition of dataThreshold --- src/main/sensors/gyroanalyse.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index ab89dd170..814f8a31f 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -263,7 +263,6 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, float fftWeightedSum = 0; float dataAvg = 0; float dataMax = 0; - float dataThreshold; bool fftPeakDetected = false; bool fftPeakFinished = false; @@ -280,7 +279,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, dataAvg = dataAvg / FFT_BIN_COUNT; //peak, once increasing, must be more than 80% above average and 1.4 times average - dataThreshold = MAX(1.4f * dataAvg, (0.8f * dataMax + 0.2f * dataAvg)); + float dataThreshold = MAX(1.4f * dataAvg, (0.8f * dataMax + 0.2f * dataAvg)); // iterate over fft data and calculate weighted indices fftPeakDetected = false; for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { From 9cd5bcd7a38fd02a60550180db2fac8f5caeaf0f Mon Sep 17 00:00:00 2001 From: mikeller Date: Thu, 16 Aug 2018 23:01:43 +1200 Subject: [PATCH 06/10] Fixed tests / SITL build. --- src/main/sensors/gyro.h | 10 +++++++++- src/main/sensors/gyroanalyse.h | 8 ++------ 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 8b711cfa4..7e27b0aff 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -21,11 +21,19 @@ #pragma once #include "common/axis.h" +#include "common/filter.h" #include "common/time.h" -#include "pg/pg.h" + #include "drivers/bus.h" #include "drivers/sensor.h" +#include "pg/pg.h" + +typedef union gyroDynamicFilter_u { + pt1Filter_t pt1FilterState; + biquadFilter_t biquadFilterState; +} gyroDynamicFilter_t; + typedef enum { GYRO_NONE = 0, GYRO_DEFAULT, diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index 4a8de2643..b599dd218 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -22,17 +22,13 @@ #include "arm_math.h" -#include "common/time.h" #include "common/filter.h" +#include "sensors/gyro.h" + // max for F3 targets #define FFT_WINDOW_SIZE 32 -typedef union gyroDynamicFilter_u { - pt1Filter_t pt1FilterState; - biquadFilter_t biquadFilterState; -} gyroDynamicFilter_t; - typedef struct gyroAnalyseState_s { // accumulator for oversampled data => no aliasing and less noise uint8_t sampleCount; From db9bc90845cc0606c9293f622a9390d9d155e2f3 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sun, 19 Aug 2018 14:16:25 +1000 Subject: [PATCH 07/10] True Peak Detection --- src/main/sensors/gyroanalyse.c | 106 +++++++++++++++------------------ 1 file changed, 47 insertions(+), 59 deletions(-) diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 814f8a31f..7edbea74e 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -44,17 +44,18 @@ // The FFT splits the frequency domain into an number of bins // A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each 31.25Hz wide // Eg [0,31), [31,62), [62, 93) etc - -// for gyro loop >= 4KHz, sample rate 2000 defines to 1000Hz, 16 bins each 62.5 Hz wide -// NB FFT_WINDOW_SIZE is defined as 32 in gyroanalyse.h +// for gyro loop >= 4KHz, sample rate 2000 defines FFT range to 1000Hz, 16 bins each 62.5 Hz wide +// NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h #define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) -// start to compare 3rd to 2nd bin, ie start comparing from 77Hz, 100Hz, and 150Hz centres +// start to compare 3rd bin to 2nd bin, ie start comparing from 77Hz, 100Hz, or 150Hz centres #define FFT_BIN_OFFSET 2 +// smoothing frequency for FFT centre frequency #define DYN_NOTCH_SMOOTH_FREQ_HZ 50 // notch centre point will not go below sample rate divided by these dividers, resulting in range limits: // HIGH : 133/166-1000Hz, MEDIUM -> 89/111-666Hz, LOW -> 67/83-500Hz #define DYN_NOTCH_MIN_CENTRE_DIV 12 -// lowest allowed notch cutoff frequency 20% below minimum allowed notch +// divider to get lowest allowed notch cutoff frequency +// otherwise cutoff is user configured percentage below centre frequency #define DYN_NOTCH_MIN_CUTOFF_DIV 15 // we need 4 steps for each axis #define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) @@ -67,7 +68,6 @@ static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMaxCenterHz; static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCutoffHz; static float FAST_RAM_ZERO_INIT dynamicFilterWidthFactor; static uint8_t FAST_RAM_ZERO_INIT dynamicFilterType; - static uint8_t dynamicFilterRange; // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window @@ -132,7 +132,6 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // any init value state->centerFreq[axis] = dynamicNotchMaxCenterHz; - state->prevCenterFreq[axis] = dynamicNotchMaxCenterHz; biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); } } @@ -257,71 +256,60 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, } case STEP_CALC_FREQUENCIES: { - // 13us - // calculate FFT centreFreq + bool fftIncreased = false; + uint8_t binStart; + float dataMax = 0; + uint8_t binMax; float fftSum = 0; float fftWeightedSum = 0; - float dataAvg = 0; - float dataMax = 0; - bool fftPeakDetected = false; - bool fftPeakFinished = false; - //get average and max of bin amplitudes once they start increasing + + //for bins after initial decline, identify start bin and max bin for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { - if (fftPeakDetected || (state->fftData[i] > state->fftData[i - 1])) { - dataAvg += state->fftData[i]; - fftPeakDetected = true; + if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) { + if (!fftIncreased) { + binStart = i; // first up-step bin + } + fftIncreased = true; if (state->fftData[i] > dataMax) { dataMax = state->fftData[i]; + binMax = i; // tallest bin } } } - dataAvg = dataAvg / FFT_BIN_COUNT; - - //peak, once increasing, must be more than 80% above average and 1.4 times average - float dataThreshold = MAX(1.4f * dataAvg, (0.8f * dataMax + 0.2f * dataAvg)); - // iterate over fft data and calculate weighted indices - fftPeakDetected = false; - for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { - const float data = state->fftData[i]; - const float dataPrev = state->fftData[i - 1]; - // include bins only after first peak detected - if (!fftPeakFinished) { - // peak must exceed thresholds and come after an increase in bin height - if (fftPeakDetected || (data > dataPrev && data > dataThreshold)) { - // add current bin - float cubedData = data * data * data; - // indicate peak detected - if (!fftPeakDetected) { - fftPeakDetected = true; - // accumulate previous bin - cubedData += dataPrev * dataPrev * dataPrev; - } - // terminate when peak ends ie data falls below average - if (data < dataAvg) { - fftPeakFinished = true; - } - //calculate sums - fftSum += cubedData; - // calculate weighted index starting at 1, not 0 - fftWeightedSum += cubedData * (i + 1); - } + // accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak + float cubedData = state->fftData[binMax] * state->fftData[binMax] * state->fftData[binMax]; + fftSum = cubedData; + fftWeightedSum += cubedData * (binMax + 1); + // accumulate upper shoulder + for (int i = binMax; i < FFT_BIN_COUNT - 1; i++) { + if (state->fftData[i] > state->fftData[i + 1]) { + cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i]; + fftSum += cubedData; + fftWeightedSum += cubedData * (i + 1); + } else { + break; + } + } + // accumulate lower shoulder + for (int i = binMax; i > binStart + 1; i--) { + if (state->fftData[i] > state->fftData[i - 1]) { + cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i]; + fftSum += cubedData; + fftWeightedSum += cubedData * (i + 1); + } else { + break; } } - // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz) - // if no peak, go to highest point to minimise delay float centerFreq = dynamicNotchMaxCenterHz; float fftMeanIndex = 0; - if (fftSum > 0) { - // idx was shifted by 1 to start at 1, not 0 - fftMeanIndex = (fftWeightedSum / fftSum) - 1; - // the index points at the center frequency of each bin so index 0 is actually 16.125Hz - centerFreq = fftMeanIndex * fftResolution; - } else { - centerFreq = state->prevCenterFreq[state->updateAxis]; - } - state->prevCenterFreq[state->updateAxis] = centerFreq; + // idx was shifted by 1 to start at 1, not 0 + fftMeanIndex = (fftWeightedSum / fftSum) - 1; + // the index points at the center frequency of each bin so index 0 is actually 16.125Hz + centerFreq = fftMeanIndex * fftResolution; + + // constrain and low-pass smooth centre frequency centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz); centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq); centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz); @@ -334,8 +322,8 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, if (state->updateAxis == 1) { DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]); } + // Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); - break; } case STEP_UPDATE_FILTERS: From 3ddc1c5be66821e1478ef985c1198aeb27dd11f0 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Mon, 20 Aug 2018 02:46:56 +1000 Subject: [PATCH 08/10] fixes, initialise, avoid div by zero --- src/main/sensors/gyroanalyse.c | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 7edbea74e..ee17e7d6a 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -132,6 +132,7 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // any init value state->centerFreq[axis] = dynamicNotchMaxCenterHz; + state->prevCenterFreq[axis] = dynamicNotchMaxCenterHz; biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); } } @@ -257,20 +258,16 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, case STEP_CALC_FREQUENCIES: { bool fftIncreased = false; - uint8_t binStart; float dataMax = 0; - uint8_t binMax; - float fftSum = 0; - float fftWeightedSum = 0; - - + uint8_t binStart = 0; + uint8_t binMax = 0; //for bins after initial decline, identify start bin and max bin for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) { if (!fftIncreased) { binStart = i; // first up-step bin + fftIncreased = true; } - fftIncreased = true; if (state->fftData[i] > dataMax) { dataMax = state->fftData[i]; binMax = i; // tallest bin @@ -279,8 +276,8 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, } // accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak float cubedData = state->fftData[binMax] * state->fftData[binMax] * state->fftData[binMax]; - fftSum = cubedData; - fftWeightedSum += cubedData * (binMax + 1); + float fftSum = cubedData; + float fftWeightedSum = cubedData * (binMax + 1); // accumulate upper shoulder for (int i = binMax; i < FFT_BIN_COUNT - 1; i++) { if (state->fftData[i] > state->fftData[i + 1]) { @@ -305,10 +302,13 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, float centerFreq = dynamicNotchMaxCenterHz; float fftMeanIndex = 0; // idx was shifted by 1 to start at 1, not 0 - fftMeanIndex = (fftWeightedSum / fftSum) - 1; - // the index points at the center frequency of each bin so index 0 is actually 16.125Hz - centerFreq = fftMeanIndex * fftResolution; - + if (fftSum > 0) { + fftMeanIndex = (fftWeightedSum / fftSum) - 1; + // the index points at the center frequency of each bin so index 0 is actually 16.125Hz + centerFreq = fftMeanIndex * fftResolution; + } else { + centerFreq = state->prevCenterFreq[state->updateAxis]; + } // constrain and low-pass smooth centre frequency centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz); centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq); From 759007214b21e06122c1c496d0fba7001dad5bcb Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Mon, 20 Aug 2018 11:34:13 +1000 Subject: [PATCH 09/10] remove PT1 warning: In file included from ./src/main/sensors/gyro.c:1022:0: ./src/main/sensors/gyro_filter_impl.h: In function 'filterGyro': ./src/main/sensors/gyro_filter_impl.h:18:15: warning: variable 'gyroDataForAnalysis' set but not used --- src/main/interface/settings.c | 1 - src/main/sensors/gyro.c | 46 ++++++++--------------------- src/main/sensors/gyro.h | 6 ---- src/main/sensors/gyro_filter_impl.h | 4 +-- src/main/sensors/gyroanalyse.c | 30 +++++-------------- src/main/sensors/gyroanalyse.h | 5 +--- 6 files changed, 23 insertions(+), 69 deletions(-) diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 7b749a677..532378a5b 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -521,7 +521,6 @@ const clivalue_t valueTable[] = { #endif #if defined(USE_GYRO_DATA_ANALYSE) { "dyn_fft_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FFT_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_fft_location) }, - { "dyn_filter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_type) }, { "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) }, { "dyn_filter_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) }, #endif diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index cb412bd0d..d02b1cdeb 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -132,8 +132,8 @@ typedef struct gyroSensor_s { filterApplyFnPtr notchFilter2ApplyFn; biquadFilter_t notchFilter2[XYZ_AXIS_COUNT]; - filterApplyFnPtr dynFilterApplyFn; - gyroDynamicFilter_t dynFilter[XYZ_AXIS_COUNT]; + filterApplyFnPtr notchFilterDynApplyFn; + biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; // overflow and recovery timeUs_t overflowTimeUs; @@ -144,9 +144,8 @@ typedef struct gyroSensor_s { #endif // USE_YAW_SPIN_RECOVERY #ifdef USE_GYRO_DATA_ANALYSE -#define DYNAMIC_LOWPASS_DEFAULT_CUTOFF_HZ 200 -#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 400 -#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 350 +#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350 +#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300 gyroAnalyseState_t gyroAnalyseState; #endif @@ -207,7 +206,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_offset_yaw = 0, .yaw_spin_recovery = true, .yaw_spin_threshold = 1950, - .dyn_filter_type = FILTER_BIQUAD, .dyn_filter_width_percent = 40, .dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS, .dyn_filter_range = DYN_FILTER_RANGE_MEDIUM, @@ -749,35 +747,15 @@ static bool isDynamicFilterActive(void) return feature(FEATURE_DYNAMIC_FILTER); } -static void gyroInitFilterDynamic(gyroSensor_t *gyroSensor) +static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) { - gyroSensor->dynFilterApplyFn = nullFilterApply; + gyroSensor->notchFilterDynApplyFn = nullFilterApply; if (isDynamicFilterActive()) { - switch (gyroConfig()->dyn_filter_type) { - case FILTER_PT1: { - const float gyroDt = gyro.targetLooptime * 1e-6f; - const float gain = pt1FilterGain(DYNAMIC_LOWPASS_DEFAULT_CUTOFF_HZ, gyroDt); - - gyroSensor->dynFilterApplyFn = (filterApplyFnPtr) pt1FilterApply; - - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - pt1FilterInit(&gyroSensor->dynFilter[axis].pt1FilterState, gain); - } - - break; - } - case FILTER_BIQUAD: { - const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); - - gyroSensor->dynFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1; - - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInit(&gyroSensor->dynFilter[axis].biquadFilterState, DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); - } - - break; - } + gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2 + 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(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH); } } } @@ -807,7 +785,7 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); #ifdef USE_GYRO_DATA_ANALYSE - gyroInitFilterDynamic(gyroSensor); + gyroInitFilterDynamicNotch(gyroSensor); #endif } @@ -1102,7 +1080,7 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { - gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->dynFilter); + gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn); } #endif } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 7e27b0aff..c22c28a2b 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -29,11 +29,6 @@ #include "pg/pg.h" -typedef union gyroDynamicFilter_u { - pt1Filter_t pt1FilterState; - biquadFilter_t biquadFilterState; -} gyroDynamicFilter_t; - typedef enum { GYRO_NONE = 0, GYRO_DEFAULT, @@ -116,7 +111,6 @@ typedef struct gyroConfig_s { uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second - uint8_t dyn_filter_type; uint8_t dyn_filter_width_percent; uint8_t dyn_fft_location; // before or after static filters uint8_t dyn_filter_range; // ignore any FFT bin below this threshold diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index fdbc8cd26..ef96c7a61 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -35,8 +35,8 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis)); } - gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroDataForAnalysis); - gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf); + gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf); + gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); } #endif diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index ee17e7d6a..845cafbd4 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -67,7 +67,6 @@ static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCenterHz; static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMaxCenterHz; static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCutoffHz; static float FAST_RAM_ZERO_INIT dynamicFilterWidthFactor; -static uint8_t FAST_RAM_ZERO_INIT dynamicFilterType; static uint8_t dynamicFilterRange; // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window @@ -83,7 +82,6 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) gyroAnalyseInitialized = true; #endif - dynamicFilterType = gyroConfig()->dyn_filter_type; dynamicFilterRange = gyroConfig()->dyn_filter_range; fftSamplingRateHz = 1000; @@ -142,12 +140,12 @@ void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float state->oversampledGyroAccumulator[axis] += sample; } -static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter); +static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn); /* * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function */ -void gyroDataAnalyse(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter) +void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) { // samples should have been pushed by `gyroDataAnalysePush` // if gyro sampling is > 1kHz, accumulate multiple samples @@ -176,7 +174,7 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter) // calculate FFT and update filters if (state->updateTicks > 0) { - gyroDataAnalyseUpdate(state, dynFilter); + gyroDataAnalyseUpdate(state, notchFilterDyn); --state->updateTicks; } } @@ -190,7 +188,7 @@ void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t /* * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds */ -static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter) +static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) { enum { STEP_ARM_CFFT_F32, @@ -329,22 +327,10 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, case STEP_UPDATE_FILTERS: { // 7us - switch (dynamicFilterType) { - case FILTER_PT1: { - const int cutoffFreq = state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor; - const float gyroDt = gyro.targetLooptime * 1e-6f; - const float gain = pt1FilterGain(cutoffFreq, gyroDt); - pt1FilterUpdateCutoff(&dynFilter[state->updateAxis].pt1FilterState, gain); - break; - } - case FILTER_BIQUAD: { - // calculate cutoffFreq and notch Q, update notch filter - const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor, dynamicNotchMinCutoffHz); - const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq); - biquadFilterUpdate(&dynFilter[state->updateAxis].biquadFilterState, state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH); - break; - } - } + // calculate cutoffFreq and notch Q, update notch filter + const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor, dynamicNotchMinCutoffHz); + const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq); + biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index b599dd218..a1b8e7a17 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -36,9 +36,6 @@ typedef struct gyroAnalyseState_s { float maxSampleCountRcp; float oversampledGyroAccumulator[XYZ_AXIS_COUNT]; - // filter for downsampled accumulated gyro - biquadFilter_t gyroBandpassFilter[XYZ_AXIS_COUNT]; - // downsampled gyro data circular buffer for frequency analysis uint8_t circularBufferIdx; float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; @@ -61,4 +58,4 @@ STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlyi void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime); void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample); -void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, gyroDynamicFilter_t *dynFilter); +void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn); From 47e4d3d68d086dfda74620a5ed19d2f38df43734 Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 20 Aug 2018 14:32:21 +1200 Subject: [PATCH 10/10] Fixed warning. --- src/main/sensors/gyro.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index d02b1cdeb..427720bcd 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -1018,7 +1018,7 @@ static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t current #endif // USE_YAW_SPIN_RECOVERY #define GYRO_FILTER_FUNCTION_NAME filterGyro -#define GYRO_FILTER_DEBUG_SET(...) +#define GYRO_FILTER_DEBUG_SET(mode, index, value) { UNUSED(mode); UNUSED(index); UNUSED(value); } #include "gyro_filter_impl.h" #undef GYRO_FILTER_FUNCTION_NAME #undef GYRO_FILTER_DEBUG_SET