diff --git a/src/main/flight/dynamic_gyro_notch.c b/src/main/flight/dynamic_gyro_notch.c index 7e6c12afbc..fa4c48e913 100644 --- a/src/main/flight/dynamic_gyro_notch.c +++ b/src/main/flight/dynamic_gyro_notch.c @@ -26,6 +26,8 @@ #ifdef USE_DYNAMIC_FILTERS +FILE_COMPILE_FOR_SPEED + #include #include "dynamic_gyro_notch.h" #include "fc/config.h" @@ -33,7 +35,12 @@ #include "sensors/gyro.h" void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) { - state->filtersApplyFn = nullFilterApply; + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + state->filtersApplyFn[axis][i] = nullFilterApply; + } + } state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f; state->enabled = gyroConfig()->dynamicGyroNotchEnabled; @@ -45,27 +52,32 @@ void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) { */ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { //Any initial notch Q is valid sice it will be updated immediately after - biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); - biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); - biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + biquadFilterInit(&state->filters[axis][i], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH); + state->filtersApplyFn[axis][i] = (filterApplyFnPtr)biquadFilterApplyDF1; + } + } - state->filtersApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; } } -void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency) { +void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, float frequency[]) { - state->frequency[axis] = frequency; - - DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, axis, frequency); - - if (state->enabled) { - biquadFilterUpdate(&state->filters[0][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH); - biquadFilterUpdate(&state->filters[1][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH); - biquadFilterUpdate(&state->filters[2][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH); + if (axis == FD_ROLL) { + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, i, frequency[i]); + } } + if (state->enabled) { + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + // Filter update happens only if peak was detected + if (frequency[i] > 0.0f) { + biquadFilterUpdate(&state->filters[axis][i], frequency[i], state->looptime, state->dynNotchQ, FILTER_NOTCH); + } + } + } } float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input) { @@ -75,9 +87,9 @@ float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, flo * We always apply all filters. If a filter dimension is disabled, one of * the function pointers will be a null apply function */ - output = state->filtersApplyFn((filter_t *)&state->filters[axis][0], output); - output = state->filtersApplyFn((filter_t *)&state->filters[axis][1], output); - output = state->filtersApplyFn((filter_t *)&state->filters[axis][2], output); + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + output = state->filtersApplyFn[axis][i]((filter_t *)&state->filters[axis][i], output); + } return output; } diff --git a/src/main/flight/dynamic_gyro_notch.h b/src/main/flight/dynamic_gyro_notch.h index 87d923fb06..c5be6a8137 100644 --- a/src/main/flight/dynamic_gyro_notch.h +++ b/src/main/flight/dynamic_gyro_notch.h @@ -27,23 +27,22 @@ #include #include "common/axis.h" #include "common/filter.h" +#include "sensors/gyro.h" #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350 typedef struct dynamicGyroNotchState_s { - uint16_t frequency[XYZ_AXIS_COUNT]; + // uint16_t frequency[XYZ_AXIS_COUNT]; float dynNotchQ; float dynNotch1Ctr; float dynNotch2Ctr; uint32_t looptime; uint8_t enabled; - /* - * Dynamic gyro filter can be 3x1, 3x2 or 3x3 depending on filter type - */ - biquadFilter_t filters[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT]; - filterApplyFnPtr filtersApplyFn; + + biquadFilter_t filters[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; + filterApplyFnPtr filtersApplyFn[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; } dynamicGyroNotchState_t; void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state); -void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency); +void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, float frequency[]); float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input); diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index 2037b4712c..7596583273 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -61,7 +61,7 @@ enum { // NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h #define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) // smoothing frequency for FFT centre frequency -#define DYN_NOTCH_SMOOTH_FREQ_HZ 50 +#define DYN_NOTCH_SMOOTH_FREQ_HZ 25 /* * Slow down gyro sample acquisition. This lowers the max frequency but increases the resolution. @@ -93,11 +93,12 @@ void gyroDataAnalyseStateInit( const uint32_t filterUpdateUs = targetLooptimeUs * STEP_COUNT * XYZ_AXIS_COUNT; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - // any init value - state->centerFreq[axis] = state->maxFrequency; - state->prevCenterFreq[axis] = state->maxFrequency; + + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + state->centerFrequency[axis][i] = state->maxFrequency; + pt1FilterInit(&state->detectedFrequencyFilter[axis][i], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs * 1e-6f); + } - biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs); } } @@ -135,18 +136,6 @@ void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1); void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable); -static uint8_t findPeakBinIndex(gyroAnalyseState_t *state) { - uint8_t peakBinIndex = state->fftStartBin; - float peakValue = 0; - for (int i = state->fftStartBin; i < FFT_BIN_COUNT; i++) { - if (state->fftData[i] > peakValue) { - peakValue = state->fftData[i]; - peakBinIndex = i; - } - } - return peakBinIndex; -} - static float computeParabolaMean(gyroAnalyseState_t *state, uint8_t peakBinIndex) { float preciseBin = peakBinIndex; @@ -190,38 +179,78 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state) // 8us arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT); - //Find peak frequency - uint8_t peakBin = findPeakBinIndex(state); + //Zero the data structure + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + state->peaks[i].bin = 0; + state->peaks[i].value = 0.0f; + } - // Failsafe to ensure the last bin is not a peak bin - peakBin = constrain(peakBin, state->fftStartBin, FFT_BIN_COUNT - 1); + // Find peaks + for (int bin = (state->fftStartBin + 1); bin < FFT_BIN_COUNT - 1; bin++) { + /* + * Peak is defined if the current bin is greater than the previous bin and the next bin + */ + if ( + state->fftData[bin] > state->fftData[bin - 1] && + state->fftData[bin] > state->fftData[bin + 1] + ) { + /* + * We are only interested in N biggest peaks + * Check previously found peaks and update the structure if necessary + */ + for (int p = 0; p < DYN_NOTCH_PEAK_COUNT; p++) { + if (state->fftData[bin] > state->peaks[p].value) { + for (int k = DYN_NOTCH_PEAK_COUNT - 1; k > p; k--) { + state->peaks[k] = state->peaks[k - 1]; + } + state->peaks[p].bin = bin; + state->peaks[p].value = state->fftData[bin]; + break; + } + } + bin++; // If bin is peak, next bin can't be peak => jump it + } + } - /* - * Calculate center frequency using the parabola method - */ - float preciseBin = computeParabolaMean(state, peakBin); - float peakFrequency = preciseBin * state->fftResolution; + // Sort N biggest peaks in ascending bin order (example: 3, 8, 25, 0, 0, ..., 0) + for (int p = DYN_NOTCH_PEAK_COUNT - 1; p > 0; p--) { + for (int k = 0; k < p; k++) { + // Swap peaks but ignore swapping void peaks (bin = 0). This leaves + // void peaks at the end of peaks array without moving them + if (state->peaks[k].bin > state->peaks[k + 1].bin && state->peaks[k + 1].bin != 0) { + peak_t temp = state->peaks[k]; + state->peaks[k] = state->peaks[k + 1]; + state->peaks[k + 1] = temp; + } + } + } - peakFrequency = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], peakFrequency); - peakFrequency = constrainf(peakFrequency, state->minFrequency, state->maxFrequency); - - state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis]; - state->centerFreq[state->updateAxis] = peakFrequency; break; } case STEP_UPDATE_FILTERS_AND_HANNING: { - // 7us - // calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004) - if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) { - /* - * Filters will be updated inside dynamicGyroNotchFiltersUpdate() - */ - state->filterUpdateExecute = true; - state->filterUpdateAxis = state->updateAxis; - state->filterUpdateFrequency = state->centerFreq[state->updateAxis]; + + /* + * Update frequencies + */ + for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) { + + if (state->peaks[i].bin > 0) { + const int bin = constrain(state->peaks[i].bin, state->fftStartBin, FFT_BIN_COUNT - 1); + float frequency = computeParabolaMean(state, bin) * state->fftResolution; + + state->centerFrequency[state->updateAxis][i] = pt1FilterApply(&state->detectedFrequencyFilter[state->updateAxis][i], frequency); + } else { + state->centerFrequency[state->updateAxis][i] = 0.0f; + } } + /* + * Filters will be updated inside dynamicGyroNotchFiltersUpdate() + */ + state->filterUpdateExecute = true; + state->filterUpdateAxis = state->updateAxis; + //Switch to the next axis state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h index 0d95be42cc..2cfd19389f 100644 --- a/src/main/flight/gyroanalyse.h +++ b/src/main/flight/gyroanalyse.h @@ -31,6 +31,11 @@ */ #define FFT_WINDOW_SIZE 64 +typedef struct peak_s { + int bin; + float value; +} peak_t; + typedef struct gyroAnalyseState_s { // accumulator for oversampled data => no aliasing and less noise float currentSample[XYZ_AXIS_COUNT]; @@ -40,7 +45,6 @@ typedef struct gyroAnalyseState_s { float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; // update state machine step information - uint8_t updateTicks; uint8_t updateStep; uint8_t updateAxis; @@ -48,12 +52,15 @@ typedef struct gyroAnalyseState_s { float fftData[FFT_WINDOW_SIZE]; float rfftData[FFT_WINDOW_SIZE]; - biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT]; - uint16_t centerFreq[XYZ_AXIS_COUNT]; - uint16_t prevCenterFreq[XYZ_AXIS_COUNT]; + pt1Filter_t detectedFrequencyFilter[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; + float centerFrequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT]; + + peak_t peaks[DYN_NOTCH_PEAK_COUNT]; + bool filterUpdateExecute; uint8_t filterUpdateAxis; uint16_t filterUpdateFrequency; + uint16_t fftSamplingRateHz; uint8_t fftStartBin; float fftResolution; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index f09b826c22..22a1ee5521 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -458,8 +458,8 @@ void FAST_CODE NOINLINE gyroFilter() if (gyroAnalyseState.filterUpdateExecute) { dynamicGyroNotchFiltersUpdate( &dynamicGyroNotchState, - gyroAnalyseState.filterUpdateAxis, - gyroAnalyseState.filterUpdateFrequency + gyroAnalyseState.filterUpdateAxis, + gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis] ); } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 63e5ab99da..cbc4d72cda 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -24,6 +24,11 @@ #include "config/parameter_group.h" #include "drivers/sensor.h" +/* + * Number of peaks to detect with Dynamic Notch Filter aka Matrixc Filter. This is equal to the number of dynamic notch filters + */ +#define DYN_NOTCH_PEAK_COUNT 3 + typedef enum { GYRO_NONE = 0, GYRO_AUTODETECT,