rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Made gyroAnalyse reentrant

This commit is contained in:
Andrey Mironov 2018-07-23 13:53:00 +03:00
parent 805b2f4b1e
commit 02eccab75b
4 changed files with 177 additions and 157 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/main/sensors/gyro.c
View File

@ -72,7 +72,9 @@
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"
#ifdef USE_GYRO_DATA_ANALYSE
#include "sensors/gyroanalyse.h" #include "sensors/gyroanalyse.h"
#endif
#include "sensors/sensors.h" #include "sensors/sensors.h"
#ifdef USE_HARDWARE_REVISION_DETECTION #ifdef USE_HARDWARE_REVISION_DETECTION
@ -140,6 +142,10 @@ typedef struct gyroSensor_s {
timeUs_t yawSpinTimeUs; timeUs_t yawSpinTimeUs;
bool yawSpinDetected; bool yawSpinDetected;
#endif // USE_YAW_SPIN_RECOVERY #endif // USE_YAW_SPIN_RECOVERY
#ifdef USE_GYRO_DATA_ANALYSE
gyroAnalyseState_t gyroAnalyseState;
#endif
} gyroSensor_t; } gyroSensor_t;
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1; STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
@ -503,8 +509,9 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
gyroInitSensorFilters(gyroSensor); gyroInitSensorFilters(gyroSensor);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseInit(gyro.targetLooptime); gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime);
#endif #endif
return true; return true;
} }
@ -520,6 +527,10 @@ bool gyroInit(void)
} }
#endif #endif
#ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseInit();
#endif
switch (debugMode) { switch (debugMode) {
case DEBUG_FFT: case DEBUG_FFT:
case DEBUG_FFT_FREQ: case DEBUG_FFT_FREQ:
@ -1066,9 +1077,10 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
} else { } else {
filterGyroDebug(gyroSensor, sampleDeltaUs); filterGyroDebug(gyroSensor, sampleDeltaUs);
} }
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
gyroDataAnalyse(gyroSensor->notchFilterDyn); gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn);
} }
#endif #endif
} }

2
src/main/sensors/gyro_filter_impl.h
View File

@ -24,7 +24,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) { if (isDynamicFilterActive()) {
gyroDataAnalysePush(axis, gyroADCf); gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
if (axis == X) { if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch

253
src/main/sensors/gyroanalyse.c
View File

@ -28,8 +28,6 @@
#include "platform.h" #include "platform.h"
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
#include "arm_math.h"
#include "build/debug.h" #include "build/debug.h"
#include "common/filter.h" #include "common/filter.h"
@ -47,141 +45,121 @@
// 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 with a width 31.25Hz
// Eg [0,31), [31,62), [62, 93) etc // 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_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 // 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_BIN_OFFSET 1
#define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4) // centre frequency of bandpass that constrains input to FFT // analyse up to 666Hz, 16 bins each 41.625 Hz wide
#define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE) // hz per bin #define FFT_SAMPLING_RATE_HZ 1333
#define FFT_MIN_BIN_RISE 2 // following bin must be at least 2 times previous to indicate start of peak // centre frequency of bandpass that constrains input to FFT
#define DYN_NOTCH_SMOOTH_FREQ_HZ 60 // lowpass frequency for smoothing notch centre point #define FFT_BPF_HZ (FFT_SAMPLING_RATE_HZ / 4)
#define DYN_NOTCH_MIN_CENTRE_HZ 125 // notch centre point will not go below this, must be greater than cutoff, mid of bottom bin // Hz per bin
#define DYN_NOTCH_MAX_CENTRE_HZ (FFT_SAMPLING_RATE_HZ / 2) // maximum notch centre frequency limited by nyquist #define FFT_RESOLUTION ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE)
#define DYN_NOTCH_MIN_CUTOFF_HZ 105 // lowest allowed notch cutoff frequency // following bin must be at least 2 times previous to indicate start of peak
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis #define FFT_MIN_BIN_RISE 2
static FAST_RAM_ZERO_INIT uint16_t fftSamplingCount; // lowpass frequency for smoothing notch centre point
#define DYN_NOTCH_SMOOTH_FREQ_HZ 60
// gyro data used for frequency analysis // notch centre point will not go below this, must be greater than cutoff, mid of bottom bin
static float FAST_RAM_ZERO_INIT gyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; #define DYN_NOTCH_MIN_CENTRE_HZ 125
// maximum notch centre frequency limited by Nyquist
static FAST_RAM_ZERO_INIT arm_rfft_fast_instance_f32 fftInstance; #define DYN_NOTCH_MAX_CENTRE_HZ (FFT_SAMPLING_RATE_HZ / 2)
static FAST_RAM_ZERO_INIT float fftData[FFT_WINDOW_SIZE]; // lowest allowed notch cutoff frequency
static FAST_RAM_ZERO_INIT float rfftData[FFT_WINDOW_SIZE]; #define DYN_NOTCH_MIN_CUTOFF_HZ 105
static FAST_RAM_ZERO_INIT gyroFftData_t fftResult[XYZ_AXIS_COUNT]; // we need 4 steps for each axis
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
// 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];
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window // 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 hanningWindow[FFT_WINDOW_SIZE];
static FAST_RAM_ZERO_INIT float dynamicNotchCutoff; static FAST_RAM_ZERO_INIT float dynamicNotchCutoff;
void initHanning(void) void gyroDataAnalyseInit(void)
{ {
for (int i = 0; i < FFT_WINDOW_SIZE; i++) { 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) void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
{
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)
{ {
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs; const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
fftSamplingCount = samplingFrequency / FFT_SAMPLING_RATE_HZ; state->maxSampleCount = samplingFrequency / FFT_SAMPLING_RATE_HZ;
arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE); state->maxSampleCountRcp = 1.f / state->maxSampleCount;
initGyroData(); arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE);
initHanning();
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls // 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 // 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 // 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); const float looptime = MAX(1000000u / FFT_SAMPLING_RATE_HZ, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fftResult[axis].centerFreq = 200; // any init value // any init value
biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime); state->centerFreq[axis] = 200;
biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE_HZ, 0.01f * gyroConfig()->dyn_notch_quality, FILTER_BPF); 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 void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample)
const gyroFftData_t *gyroFftData(int axis)
{ {
return &fftResult[axis]; state->oversampledGyroAccumulator[axis] += sample;
} }
static FAST_RAM_ZERO_INIT float fftAcc[XYZ_AXIS_COUNT]; static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn);
void gyroDataAnalysePush(const int axis, const float sample)
{
fftAcc[axis] += sample;
}
/* /*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function * 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` // samples should have been pushed by `gyroDataAnalysePush`
// if gyro sampling is > 1kHz, accumulate multiple samples // if gyro sampling is > 1kHz, accumulate multiple samples
fftAccCount++; state->sampleCount++;
// this runs at 1kHz // this runs at 1kHz
if (fftAccCount == fftSamplingCount) { if (state->sampleCount == state->maxSampleCount) {
fftAccCount = 0; state->sampleCount = 0;
//calculate mean value of accumulated samples // calculate mean value of accumulated samples
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
float sample = fftAcc[axis] / fftSamplingCount; float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
sample = biquadFilterApply(&fftGyroFilter[axis], sample); sample = biquadFilterApply(&state->gyroBandpassFilter[axis], sample);
gyroData[axis][fftIdx] = sample;
if (axis == 0) state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
if (axis == 0) {
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample)); DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
fftAcc[axis] = 0;
} }
fftIdx = (fftIdx + 1) % FFT_WINDOW_SIZE; state->oversampledGyroAccumulator[axis] = 0;
}
state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE;
// We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value // We need DYN_NOTCH_CALC_TICKS 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 // calculate FFT and update filters
if (gyroDataAnalyseUpdateTicks > 0) { if (state->updateTicks > 0) {
gyroDataAnalyseUpdate(notchFilterDyn); gyroDataAnalyseUpdate(state, notchFilterDyn);
--gyroDataAnalyseUpdateTicks; --state->updateTicks;
} }
} }
void stage_rfft_f32(arm_rfft_fast_instance_f32 * S, float32_t * p, float32_t * pOut); 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_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_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_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); void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable);
typedef enum { /*
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
*/
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
{
enum {
STEP_ARM_CFFT_F32, STEP_ARM_CFFT_F32,
STEP_BITREVERSAL, STEP_BITREVERSAL,
STEP_STAGE_RFFT_F32, STEP_STAGE_RFFT_F32,
@ -190,37 +168,31 @@ typedef enum {
STEP_UPDATE_FILTERS, STEP_UPDATE_FILTERS,
STEP_HANNING, STEP_HANNING,
STEP_COUNT STEP_COUNT
} UpdateStep_e; };
/* arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint);
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
*/
void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
{
static int axis;
static int step;
arm_cfft_instance_f32 * Sint = &(fftInstance.Sint);
uint32_t startTime = 0; uint32_t startTime = 0;
if (debugMode == (DEBUG_FFT_TIME)) if (debugMode == (DEBUG_FFT_TIME)) {
startTime = micros(); startTime = micros();
}
DEBUG_SET(DEBUG_FFT_TIME, 0, step); DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep);
switch (step) { switch (state->updateStep) {
case STEP_ARM_CFFT_F32: case STEP_ARM_CFFT_F32:
{ {
switch (FFT_BIN_COUNT) { switch (FFT_BIN_COUNT) {
case 16: case 16:
// 16us // 16us
arm_cfft_radix8by2_f32(Sint, fftData); arm_cfft_radix8by2_f32(Sint, state->fftData);
break; break;
case 32: case 32:
// 35us // 35us
arm_cfft_radix8by4_f32(Sint, fftData); arm_cfft_radix8by4_f32(Sint, state->fftData);
break; break;
case 64: case 64:
// 70us // 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; break;
} }
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
@ -229,25 +201,25 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
case STEP_BITREVERSAL: case STEP_BITREVERSAL:
{ {
// 6us // 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); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
step++; state->updateStep++;
FALLTHROUGH; FALLTHROUGH;
} }
case STEP_STAGE_RFFT_F32: case STEP_STAGE_RFFT_F32:
{ {
// 14us // 14us
// this does not work in place => fftData AND rfftData needed // 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); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break; break;
} }
case STEP_ARM_CMPLX_MAG_F32: case STEP_ARM_CMPLX_MAG_F32:
{ {
// 8us // 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); DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
step++; state->updateStep++;
FALLTHROUGH; FALLTHROUGH;
} }
case STEP_CALC_FREQUENCIES: case STEP_CALC_FREQUENCIES:
@ -256,40 +228,48 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
// calculate FFT centreFreq // calculate FFT centreFreq
float fftSum = 0; float fftSum = 0;
float fftWeightedSum = 0; float fftWeightedSum = 0;
fftResult[axis].maxVal = 0;
bool fftIncreasing = false; 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++) { for (int i = 1 + FFT_BIN_OFFSET; i < FFT_BIN_COUNT; i++) {
if (!fftIncreasing && (fftData[i] < fftData[i-1] * FFT_MIN_BIN_RISE)) { const float data = state->fftData[i];
// do nothing unless has increased at some point const float prevData = state->fftData[i - 1];
} else {
cubedData = fftData[i] * fftData[i] * fftData[i]; //more weight on higher peaks if (fftIncreasing || data > prevData * FFT_MIN_BIN_RISE) {
if (!fftIncreasing){ float cubedData = data * data * data;
cubedData += fftData[i-1] * fftData[i-1] * fftData[i-1]; //add previous bin before first rise
} // add previous bin before first rise
fftSum += cubedData; if (!fftIncreasing) {
fftWeightedSum += cubedData * (i + 1); // calculate weighted index starting at 1, not 0 cubedData += prevData * prevData * prevData;
fftIncreasing = true; fftIncreasing = true;
} }
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) // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
float centerFreq; // if no peak, go to highest point to minimise delay
float fftMeanIndex; float centerFreq = DYN_NOTCH_MAX_CENTRE_HZ;
float fftMeanIndex = 0;
if (fftSum > 0) { if (fftSum > 0) {
// idx was shifted by 1 to start at 1, not 0 // idx was shifted by 1 to start at 1, not 0
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 // 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); 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; centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
if (axis == 0) { state->centerFreq[state->updateAxis] = centerFreq;
if (state->updateAxis == 0) {
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100)); 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); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break; break;
} }
@ -297,13 +277,13 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
{ {
// 7us // 7us
// calculate cutoffFreq and notch Q, update notch filter // calculate cutoffFreq and notch Q, update notch filter
float cutoffFreq = fmax(fftResult[axis].centerFreq * dynamicNotchCutoff, DYN_NOTCH_MIN_CUTOFF_HZ); const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicNotchCutoff, DYN_NOTCH_MIN_CUTOFF_HZ);
float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq); const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq);
biquadFilterUpdate(&notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH); biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
axis = (axis + 1) % 3; state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
step++; state->updateStep++;
FALLTHROUGH; FALLTHROUGH;
} }
case STEP_HANNING: case STEP_HANNING:
@ -311,16 +291,17 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
// 5us // 5us
// apply hanning window to gyro samples and store result in fftData // apply hanning window to gyro samples and store result in fftData
// hanning starts and ends with 0, could be skipped for minor speed improvement // hanning starts and ends with 0, could be skipped for minor speed improvement
uint8_t ringBufIdx = FFT_WINDOW_SIZE - fftIdx; const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx;
arm_mult_f32(&gyroData[axis][fftIdx], &hanningWindow[0], &fftData[0], ringBufIdx); arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx);
if (fftIdx > 0) if (state->circularBufferIdx > 0) {
arm_mult_f32(&gyroData[axis][0], &hanningWindow[ringBufIdx], &fftData[ringBufIdx], fftIdx); arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); 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 #endif // USE_GYRO_DATA_ANALYSE

47
src/main/sensors/gyroanalyse.h
View File

@ -20,17 +20,44 @@
#pragma once #pragma once
#include "arm_math.h"
#include "common/time.h" #include "common/time.h"
#include "common/filter.h" #include "common/filter.h"
typedef struct gyroFftData_s { // max for F3 targets
float maxVal; #define FFT_WINDOW_SIZE 32
uint16_t centerFreq;
} gyroFftData_t;
void gyroDataAnalyseInit(uint32_t targetLooptime); typedef struct gyroAnalyseState_s {
const gyroFftData_t *gyroFftData(int axis); // accumulator for oversampled data => no aliasing and less noise
struct gyroDev_s; uint8_t sampleCount;
void gyroDataAnalysePush(int axis, float sample); uint8_t maxSampleCount;
void gyroDataAnalyse(biquadFilter_t *notchFilterDyn); float maxSampleCountRcp;
void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn); 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);