From 02eccab75b1c3e212266f9f0f25cf4ad1137900c Mon Sep 17 00:00:00 2001 From: Andrey Mironov Date: Mon, 23 Jul 2018 13:53:00 +0300 Subject: [PATCH] Made gyroAnalyse reentrant --- src/main/sensors/gyro.c | 16 +- src/main/sensors/gyro_filter_impl.h | 2 +- src/main/sensors/gyroanalyse.c | 269 +++++++++++++--------------- src/main/sensors/gyroanalyse.h | 47 +++-- 4 files changed, 177 insertions(+), 157 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 42f056211..05adeb3ac 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -72,7 +72,9 @@ #include "sensors/boardalignment.h" #include "sensors/gyro.h" +#ifdef USE_GYRO_DATA_ANALYSE #include "sensors/gyroanalyse.h" +#endif #include "sensors/sensors.h" #ifdef USE_HARDWARE_REVISION_DETECTION @@ -140,6 +142,10 @@ typedef struct gyroSensor_s { timeUs_t yawSpinTimeUs; bool yawSpinDetected; #endif // USE_YAW_SPIN_RECOVERY + +#ifdef USE_GYRO_DATA_ANALYSE + gyroAnalyseState_t gyroAnalyseState; +#endif } gyroSensor_t; STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1; @@ -503,8 +509,9 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) gyroInitSensorFilters(gyroSensor); #ifdef USE_GYRO_DATA_ANALYSE - gyroDataAnalyseInit(gyro.targetLooptime); + gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime); #endif + return true; } @@ -520,6 +527,10 @@ bool gyroInit(void) } #endif +#ifdef USE_GYRO_DATA_ANALYSE + gyroDataAnalyseInit(); +#endif + switch (debugMode) { case DEBUG_FFT: case DEBUG_FFT_FREQ: @@ -1066,9 +1077,10 @@ 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); + gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn); } #endif } diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index 3a105b917..cd7430a5c 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -24,7 +24,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { - gyroDataAnalysePush(axis, gyroADCf); + 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 diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 8a2a2aeb1..236126739 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -20,7 +20,7 @@ /* 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 + * coding assistance and advice from DieHertz, Rav, eTracer * test pilots icr4sh, UAV Tech, Flint723 */ #include @@ -28,8 +28,6 @@ #include "platform.h" #ifdef USE_GYRO_DATA_ANALYSE -#include "arm_math.h" - #include "build/debug.h" #include "common/filter.h" @@ -47,180 +45,154 @@ // 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_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_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_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 +// compare 1 + this offset FFT bins for peak, ie if 1 start 2.5 * 41.655 or about 104Hz +#define FFT_BIN_OFFSET 1 +// analyse up to 666Hz, 16 bins each 41.625 Hz wide +#define FFT_SAMPLING_RATE_HZ 1333 +// centre frequency of bandpass that constrains input to FFT +#define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4) +// Hz per bin +#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 -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]; - -static FAST_RAM_ZERO_INIT arm_rfft_fast_instance_f32 fftInstance; -static FAST_RAM_ZERO_INIT float fftData[FFT_WINDOW_SIZE]; -static FAST_RAM_ZERO_INIT float rfftData[FFT_WINDOW_SIZE]; -static FAST_RAM_ZERO_INIT gyroFftData_t fftResult[XYZ_AXIS_COUNT]; - -// use a circular buffer for the last FFT_WINDOW_SIZE samples -static FAST_RAM_ZERO_INIT uint16_t fftIdx; - -// bandpass filter gyro data -static FAST_RAM_ZERO_INIT biquadFilter_t fftGyroFilter[XYZ_AXIS_COUNT]; - -// filter for smoothing frequency estimation -static FAST_RAM_ZERO_INIT biquadFilter_t fftFreqFilter[XYZ_AXIS_COUNT]; +// lowpass frequency for smoothing notch centre point +#define DYN_NOTCH_SMOOTH_FREQ_HZ 60 +// notch centre point will not go below this, must be greater than cutoff, mid of bottom bin +#define DYN_NOTCH_MIN_CENTRE_HZ 125 +// 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 +// we need 4 steps for each axis +#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // 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; -void initHanning(void) +void gyroDataAnalyseInit(void) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) { - hanningWindow[i] = (0.5 - 0.5 * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); + 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; } -void initGyroData(void) -{ - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - for (int i = 0; i < FFT_WINDOW_SIZE; i++) { - gyroData[axis][i] = 0; - } - } -} - -void gyroDataAnalyseInit(uint32_t targetLooptimeUs) +void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) { // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later const uint16_t samplingFrequency = 1000000 / targetLooptimeUs; - fftSamplingCount = samplingFrequency / FFT_SAMPLING_RATE_HZ; - arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE); + state->maxSampleCount = samplingFrequency / FFT_SAMPLING_RATE_HZ; + state->maxSampleCountRcp = 1.f / state->maxSampleCount; - initGyroData(); - initHanning(); + 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 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_SMOOTH_FREQ_HZ, looptime); - biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE_HZ, 0.01f * gyroConfig()->dyn_notch_quality, FILTER_BPF); + // any init value + state->centerFreq[axis] = 200; + biquadFilterInit(&state->gyroBandpassFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE_HZ, 0.01f * gyroConfig()->dyn_notch_quality, FILTER_BPF); + biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); } - - dynamicNotchCutoff = (100.0f - gyroConfig()->dyn_notch_width_percent) / 100; } -// used in OSD -const gyroFftData_t *gyroFftData(int axis) +void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample) { - return &fftResult[axis]; + state->oversampledGyroAccumulator[axis] += sample; } -static FAST_RAM_ZERO_INIT float fftAcc[XYZ_AXIS_COUNT]; -void gyroDataAnalysePush(const int axis, const float sample) -{ - fftAcc[axis] += sample; -} +static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn); /* * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function */ -void gyroDataAnalyse(biquadFilter_t *notchFilterDyn) +void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) { - // accumulator for oversampled data => no aliasing and less noise - static FAST_RAM_ZERO_INIT uint32_t fftAccCount; - static FAST_RAM_ZERO_INIT uint32_t gyroDataAnalyseUpdateTicks; - - // samples should have been pushed by `gyroDataAnalysePush` - // if gyro sampling is > 1kHz, accumulate multiple samples - fftAccCount++; + // samples should have been pushed by `gyroDataAnalysePush` + // if gyro sampling is > 1kHz, accumulate multiple samples + state->sampleCount++; // this runs at 1kHz - if (fftAccCount == fftSamplingCount) { - fftAccCount = 0; + if (state->sampleCount == state->maxSampleCount) { + state->sampleCount = 0; - //calculate mean value of accumulated samples + // calculate mean value of accumulated samples for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - float sample = fftAcc[axis] / fftSamplingCount; - sample = biquadFilterApply(&fftGyroFilter[axis], sample); - gyroData[axis][fftIdx] = sample; - if (axis == 0) + float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp; + sample = biquadFilterApply(&state->gyroBandpassFilter[axis], sample); + + state->downsampledGyroData[axis][state->circularBufferIdx] = sample; + if (axis == 0) { DEBUG_SET(DEBUG_FFT, 2, lrintf(sample)); - fftAcc[axis] = 0; + } + + state->oversampledGyroAccumulator[axis] = 0; } - fftIdx = (fftIdx + 1) % FFT_WINDOW_SIZE; + state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE; // We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value - gyroDataAnalyseUpdateTicks = DYN_NOTCH_CALC_TICKS; + state->updateTicks = DYN_NOTCH_CALC_TICKS; } // calculate FFT and update filters - if (gyroDataAnalyseUpdateTicks > 0) { - gyroDataAnalyseUpdate(notchFilterDyn); - --gyroDataAnalyseUpdateTicks; + if (state->updateTicks > 0) { + gyroDataAnalyseUpdate(state, notchFilterDyn); + --state->updateTicks; } } -void stage_rfft_f32(arm_rfft_fast_instance_f32 * S, float32_t * p, float32_t * pOut); -void arm_cfft_radix8by2_f32( arm_cfft_instance_f32 * S, float32_t * p1); -void arm_cfft_radix8by4_f32( arm_cfft_instance_f32 * S, float32_t * p1); -void arm_radix8_butterfly_f32(float32_t * pSrc, uint16_t fftLen, const float32_t * pCoef, uint16_t twidCoefModifier); -void arm_bitreversal_32(uint32_t * pSrc, const uint16_t bitRevLen, const uint16_t * pBitRevTable); - -typedef enum { - STEP_ARM_CFFT_F32, - STEP_BITREVERSAL, - STEP_STAGE_RFFT_F32, - STEP_ARM_CMPLX_MAG_F32, - STEP_CALC_FREQUENCIES, - STEP_UPDATE_FILTERS, - STEP_HANNING, - STEP_COUNT -} UpdateStep_e; +void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut); +void arm_cfft_radix8by2_f32(arm_cfft_instance_f32 *S, float32_t *p1); +void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1); +void arm_radix8_butterfly_f32(float32_t *pSrc, uint16_t fftLen, const float32_t *pCoef, uint16_t twidCoefModifier); +void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable); /* * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds */ -void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) +static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn) { - static int axis; - static int step; - arm_cfft_instance_f32 * Sint = &(fftInstance.Sint); + enum { + STEP_ARM_CFFT_F32, + STEP_BITREVERSAL, + STEP_STAGE_RFFT_F32, + STEP_ARM_CMPLX_MAG_F32, + STEP_CALC_FREQUENCIES, + STEP_UPDATE_FILTERS, + STEP_HANNING, + STEP_COUNT + }; + + arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint); uint32_t startTime = 0; - if (debugMode == (DEBUG_FFT_TIME)) + if (debugMode == (DEBUG_FFT_TIME)) { startTime = micros(); + } - DEBUG_SET(DEBUG_FFT_TIME, 0, step); - switch (step) { + DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep); + switch (state->updateStep) { case STEP_ARM_CFFT_F32: { switch (FFT_BIN_COUNT) { case 16: // 16us - arm_cfft_radix8by2_f32(Sint, fftData); + arm_cfft_radix8by2_f32(Sint, state->fftData); break; case 32: // 35us - arm_cfft_radix8by4_f32(Sint, fftData); + arm_cfft_radix8by4_f32(Sint, state->fftData); break; case 64: // 70us - arm_radix8_butterfly_f32(fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1); + arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1); break; } DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); @@ -229,25 +201,25 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) case STEP_BITREVERSAL: { // 6us - arm_bitreversal_32((uint32_t*) fftData, Sint->bitRevLength, Sint->pBitRevTable); + arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); - step++; + state->updateStep++; FALLTHROUGH; } case STEP_STAGE_RFFT_F32: { // 14us // this does not work in place => fftData AND rfftData needed - stage_rfft_f32(&fftInstance, fftData, rfftData); + stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); break; } case STEP_ARM_CMPLX_MAG_F32: { // 8us - arm_cmplx_mag_f32(rfftData, fftData, FFT_BIN_COUNT); + arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT); DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime); - step++; + state->updateStep++; FALLTHROUGH; } case STEP_CALC_FREQUENCIES: @@ -256,40 +228,48 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) // 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 + + // iterate over fft data and calculate weighted indices 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 - } + 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; + } + fftSum += cubedData; - fftWeightedSum += cubedData * (i + 1); // calculate weighted index starting at 1, not 0 - fftIncreasing = true; + // 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) - float centerFreq; - float fftMeanIndex; + // if no peak, go to highest point to minimise delay + 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 * 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) { + + centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq); + state->centerFreq[state->updateAxis] = centerFreq; + + if (state->updateAxis == 0) { DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); } - DEBUG_SET(DEBUG_FFT_FREQ, axis, fftResult[axis].centerFreq); + DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis, state->centerFreq[state->updateAxis]); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); break; } @@ -297,13 +277,13 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) { // 7us // calculate cutoffFreq and notch Q, update notch filter - float cutoffFreq = fmax(fftResult[axis].centerFreq * dynamicNotchCutoff, DYN_NOTCH_MIN_CUTOFF_HZ); - float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq); - biquadFilterUpdate(¬chFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH); + 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); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); - axis = (axis + 1) % 3; - step++; + state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT; + state->updateStep++; FALLTHROUGH; } case STEP_HANNING: @@ -311,16 +291,17 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) // 5us // apply hanning window to gyro samples and store result in fftData // hanning starts and ends with 0, could be skipped for minor speed improvement - uint8_t ringBufIdx = FFT_WINDOW_SIZE - fftIdx; - arm_mult_f32(&gyroData[axis][fftIdx], &hanningWindow[0], &fftData[0], ringBufIdx); - if (fftIdx > 0) - arm_mult_f32(&gyroData[axis][0], &hanningWindow[ringBufIdx], &fftData[ringBufIdx], fftIdx); + const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx; + arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx); + if (state->circularBufferIdx > 0) { + arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx); + } DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); } } - step = (step + 1) % STEP_COUNT; + state->updateStep = (state->updateStep + 1) % STEP_COUNT; } #endif // USE_GYRO_DATA_ANALYSE diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index 182d3135c..89d4401f3 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -20,17 +20,44 @@ #pragma once +#include "arm_math.h" + #include "common/time.h" #include "common/filter.h" -typedef struct gyroFftData_s { - float maxVal; - uint16_t centerFreq; -} gyroFftData_t; +// max for F3 targets +#define FFT_WINDOW_SIZE 32 -void gyroDataAnalyseInit(uint32_t targetLooptime); -const gyroFftData_t *gyroFftData(int axis); -struct gyroDev_s; -void gyroDataAnalysePush(int axis, float sample); -void gyroDataAnalyse(biquadFilter_t *notchFilterDyn); -void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn); +typedef struct gyroAnalyseState_s { + // accumulator for oversampled data => no aliasing and less noise + uint8_t sampleCount; + uint8_t maxSampleCount; + float maxSampleCountRcp; + float oversampledGyroAccumulator[XYZ_AXIS_COUNT]; + + // 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]; + + // update state machine step information + uint8_t updateTicks; + uint8_t updateStep; + uint8_t updateAxis; + + arm_rfft_fast_instance_f32 fftInstance; + float fftData[FFT_WINDOW_SIZE]; + float rfftData[FFT_WINDOW_SIZE]; + + biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT]; + uint16_t centerFreq[XYZ_AXIS_COUNT]; +} gyroAnalyseState_t; + +STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type); + +void gyroDataAnalyseInit(void); +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);