From 36d71d07e7c0a4f1e768be29bb6cc9757a4626d8 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sat, 21 Jul 2018 22:53:07 +1000 Subject: [PATCH] Dynamic Notch Filter Update Improves performance of the dynamic notch filter, increasing peak accuracy over a wider band of frequencies, and generally using a narrower, higher notch. Details: - FFT now operates on gyro data *after* gyro notches and lowpasses - FFT bandpass Q changed from 0.707 to 0.05, to 'open up' the FFT to a greater range of incoming frequencies - FFT centre output now ranges from about 130 to 666Hz. - ignore the lowest couple of FFT bins going into centre frequency calculation - analyse FFT bins from low to high, keep ignoring bins until a bin is found that exceeds its previous bin by a factor of 2; then start examining bins from the bin before that (stops the FFT from being biased low, or going to the lowest value if there is no notch at all). - if no bin exceeds previous by more than 2 times, ie no obvious peak, smoothly go to maximum allowed notch frequency to avoid delay (might be better to bypass filter altogether?) - dominant bin emphasised by cubing bin height before calculating mean - maximum cutoff frequency is half the highest allowable centre frequency - default notch width is +/-25% of centre, narrower than before most of the time - code tidied up - thanks to rav, Flint, UAV Tech, icr4sh, diehertz and everyone else who helped with this. --- src/main/sensors/gyro.c | 12 ++-- src/main/sensors/gyro_filter_impl.h | 15 +++-- src/main/sensors/gyroanalyse.c | 119 ++++++++++++++++++++---------------- src/main/sensors/gyroanalyse.h | 3 +- 4 files changed, 83 insertions(+), 66 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index dbe7014d1..1e288e9b9 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -520,6 +520,7 @@ bool gyroInit(void) switch (debugMode) { case DEBUG_FFT: + case DEBUG_FFT_FREQ: case DEBUG_GYRO_RAW: case DEBUG_GYRO_SCALED: case DEBUG_GYRO_FILTERED: @@ -1042,12 +1043,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens return; } -#ifdef USE_GYRO_DATA_ANALYSE - if (isDynamicFilterActive()) { - gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn); - } -#endif - const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs; accumulationLastTimeSampledUs = currentTimeUs; accumulatedMeasurementTimeUs += sampleDeltaUs; @@ -1069,6 +1064,11 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens } else { filterGyroDebug(gyroSensor, sampleDeltaUs); } +#ifdef USE_GYRO_DATA_ANALYSE + if (isDynamicFilterActive()) { + gyroDataAnalyse(gyroSensor->notchFilterDyn); + } +#endif } FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index bdccbb3ef..d0502f4ac 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -7,11 +7,18 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe // DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf)); + // apply static notch filters and software lowpass filters + gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); + gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); + gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); + #ifdef USE_GYRO_DATA_ANALYSE - // apply dynamic notch filter if (isDynamicFilterActive()) { + gyroDataAnalysePush(axis, gyroADCf); 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 } gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); if (axis == X) { @@ -19,11 +26,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe } } #endif - // apply static notch filters and software lowpass filters - gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); - gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); - gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); - gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); + // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied. GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf)); diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index a5958894b..fd7fc2180 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -18,6 +18,11 @@ * If not, see . */ +/* 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 + */ #include #include "platform.h" @@ -42,22 +47,23 @@ // 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_WINDOW_SIZE 32 // max for f3 targets -#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) -#define FFT_MIN_FREQ 100 // not interested in filtering frequencies below 100Hz -#define FFT_SAMPLING_RATE 1000 // allows analysis up to 500Hz which is more than motors create -#define FFT_MAX_FREQUENCY (FFT_SAMPLING_RATE / 2) // nyquist rate -#define FFT_BPF_HZ 200 // use a bandpass on gyro data to ignore extreme low and extreme high frequencies -#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE / FFT_WINDOW_SIZE) // hz per bin -#define DYN_NOTCH_WIDTH 100 // just an orientation and start value -#define DYN_NOTCH_CHANGERATE 60 // lower cut does not improve the performance much, higher cut makes it worse... -#define DYN_NOTCH_MIN_CUTOFF 120 // don't cut too deep into low frequencies -#define DYN_NOTCH_MAX_CUTOFF 200 // don't go above this cutoff (better filtering with "constant" delay at higher center frequencies) -#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis - -#define BIQUAD_Q 1.0f / sqrtf(2.0f) // quality factor - butterworth - -static FAST_RAM_ZERO_INIT uint16_t fftSamplingScale; +#define FFT_WINDOW_SIZE 32 // max for f3 targets +#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) +#define FFT_BIN_OFFSET 1 // compare 1 + this offset FFT bins for peak, ie if 1 start 2.5 * 41.655 or about 104Hz +#define FFT_SAMPLING_RATE_HZ 1333 // analyse up to 666Hz, 16 bins each 41.655z wide +#define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4) // centre frequency of bandpass that constrains input to FFT +#define FFT_BIQUAD_Q 0.05f // bandpass quality factor, 0.1 for steep sided bandpass +#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE) // hz per bin +#define FFT_MIN_BIN_RISE 2 // following bin must be at least 2 times previous to indicate start of peak +#define DYN_NOTCH_SMOOTH_FREQ_HZ 60 // lowpass frequency for smoothing notch centre point +#define DYN_NOTCH_MIN_CENTRE_HZ 125 // notch centre point will not go below this, must be greater than cutoff, mid of bottom bin +#define DYN_NOTCH_MAX_CENTRE_HZ (FFT_SAMPLING_RATE_HZ / 2) // maximum notch centre frequency limited by nyquist +#define DYN_NOTCH_WIDTH_PERCENT 25 // maybe adjustable via CLI? +#define DYN_NOTCH_CUTOFF ((float)(100 - DYN_NOTCH_WIDTH_PERCENT) / 100) +#define DYN_NOTCH_MIN_CUTOFF_HZ 105 // lowest allowed notch cutoff frequency +#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis + +static FAST_RAM_ZERO_INIT uint16_t fftSamplingCount; // gyro data used for frequency analysis static float FAST_RAM_ZERO_INIT gyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; @@ -99,7 +105,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) { // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later const uint16_t samplingFrequency = 1000000 / targetLooptimeUs; - fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE; + fftSamplingCount = samplingFrequency / FFT_SAMPLING_RATE_HZ; arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE); initGyroData(); @@ -108,11 +114,11 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) // 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 / FFT_SAMPLING_RATE, targetLooptimeUs * DYN_NOTCH_CALC_TICKS); + const float looptime = MAX(1000000u / FFT_SAMPLING_RATE_HZ, targetLooptimeUs * DYN_NOTCH_CALC_TICKS); for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { fftResult[axis].centerFreq = 200; // any init value - biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime); - biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF); + biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); + biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE_HZ, FFT_BIQUAD_Q, FILTER_BPF); } } @@ -122,34 +128,36 @@ const gyroFftData_t *gyroFftData(int axis) return &fftResult[axis]; } +static FAST_RAM_ZERO_INIT float fftAcc[XYZ_AXIS_COUNT]; +void gyroDataAnalysePush(const int axis, const float sample) +{ + fftAcc[axis] += sample; +} + /* * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function */ -void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) +void gyroDataAnalyse(biquadFilter_t *notchFilterDyn) { // accumulator for oversampled data => no aliasing and less noise - static FAST_RAM_ZERO_INIT float fftAcc[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT uint32_t fftAccCount; - static FAST_RAM_ZERO_INIT uint32_t gyroDataAnalyseUpdateTicks; - // if gyro sampling is > 1kHz, accumulate multiple samples - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - fftAcc[axis] += gyroDev->gyroADC[axis]; - } + // samples should have been pushed by `gyroDataAnalysePush` + // if gyro sampling is > 1kHz, accumulate multiple samples fftAccCount++; // this runs at 1kHz - if (fftAccCount == fftSamplingScale) { + if (fftAccCount == fftSamplingCount) { fftAccCount = 0; //calculate mean value of accumulated samples for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - float sample = fftAcc[axis] / fftSamplingScale; + float sample = fftAcc[axis] / fftSamplingCount; sample = biquadFilterApply(&fftGyroFilter[axis], sample); gyroData[axis][fftIdx] = sample; if (axis == 0) - DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale)); + DEBUG_SET(DEBUG_FFT, 2, lrintf(sample)); fftAcc[axis] = 0; } @@ -244,37 +252,42 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) case STEP_CALC_FREQUENCIES: { // 13us + // calculate FFT centreFreq float fftSum = 0; float fftWeightedSum = 0; - fftResult[axis].maxVal = 0; + bool fftIncreasing = false; + float cubedData; // iterate over fft data and calculate weighted indexes - float squaredData; - for (int i = 0; i < FFT_BIN_COUNT; i++) { - squaredData = fftData[i] * fftData[i]; //more weight on higher peaks - fftResult[axis].maxVal = MAX(fftResult[axis].maxVal, squaredData); - fftSum += squaredData; - fftWeightedSum += squaredData * (i + 1); // calculate weighted index starting at 1, not 0 + for (int i = 1 + FFT_BIN_OFFSET; i < FFT_BIN_COUNT; i++) { + if (!fftIncreasing && (fftData[i] < fftData[i-1] * FFT_MIN_BIN_RISE)) { + // do nothing unless has increased at some point + } else { + cubedData = fftData[i] * fftData[i] * fftData[i]; //more weight on higher peaks + if (!fftIncreasing){ + cubedData += fftData[i-1] * fftData[i-1] * fftData[i-1]; //add previous bin before first rise + } + fftSum += cubedData; + fftWeightedSum += cubedData * (i + 1); // calculate weighted index starting at 1, not 0 + fftIncreasing = true; + } } - // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz) + float centerFreq; + float fftMeanIndex; if (fftSum > 0) { // idx was shifted by 1 to start at 1, not 0 - float fftMeanIndex = (fftWeightedSum / fftSum) - 1; + fftMeanIndex = (fftWeightedSum / fftSum) - 1; // the index points at the center frequency of each bin so index 0 is actually 16.125Hz - // fftMeanIndex += 0.5; - - // don't go below the minimal cutoff frequency + 10 and don't jump around too much - float centerFreq; - centerFreq = constrain(fftMeanIndex * FFT_RESOLUTION, DYN_NOTCH_MIN_CUTOFF + 10, FFT_MAX_FREQUENCY); - centerFreq = biquadFilterApply(&fftFreqFilter[axis], centerFreq); - centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CUTOFF + 10, FFT_MAX_FREQUENCY); - fftResult[axis].centerFreq = centerFreq; - if (axis == 0) { - DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); - } + centerFreq = constrain(fftMeanIndex * FFT_RESOLUTION, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ); + } else { + centerFreq = DYN_NOTCH_MAX_CENTRE_HZ; // if no peak, go to highest point to minimise delay + } + centerFreq = biquadFilterApply(&fftFreqFilter[axis], centerFreq); + fftResult[axis].centerFreq = centerFreq; + if (axis == 0) { + DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); } - DEBUG_SET(DEBUG_FFT_FREQ, axis, fftResult[axis].centerFreq); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); break; @@ -282,8 +295,8 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) case STEP_UPDATE_FILTERS: { // 7us - // calculate new filter coefficients - float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF); + // calculate cutoffFreq and notch Q, update notch filter + float cutoffFreq = fmax(fftResult[axis].centerFreq * DYN_NOTCH_CUTOFF, DYN_NOTCH_MIN_CUTOFF_HZ); float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq); biquadFilterUpdate(¬chFilterDyn[axis], fftResult[axis].centerFreq, 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 b0565ec3f..182d3135c 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -31,5 +31,6 @@ typedef struct gyroFftData_s { void gyroDataAnalyseInit(uint32_t targetLooptime); const gyroFftData_t *gyroFftData(int axis); struct gyroDev_s; -void gyroDataAnalyse(const struct gyroDev_s *gyroDev, biquadFilter_t *notchFilterDyn); +void gyroDataAnalysePush(int axis, float sample); +void gyroDataAnalyse(biquadFilter_t *notchFilterDyn); void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn); -- 2.11.4.GIT