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

Merge pull request #6428 from DieHertz/bfdev-gyro-analyse-refactor 

Dynamic Filter refactor
This commit is contained in:
Michael Keller 2018-07-25 00:22:27 +12:00 committed by GitHub
parent 7ee45d1a73 19f5d3cd98
commit cacfebe869
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 177 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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:
@ -536,7 +547,6 @@ bool gyroInit(void)
firstArmingCalibrationWasStarted = false;
bool ret = false;
memset(&gyro, 0, sizeof(gyro));
gyroToUse = gyroConfig()->gyro_to_use;
#if defined(USE_DUAL_GYRO) && defined(GYRO_1_CS_PIN)
@ -1066,9 +1076,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
}

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
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

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

@ -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,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
// 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++;
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;
}
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
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);
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 {
/*
* 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_BITREVERSAL,
STEP_STAGE_RFFT_F32,
@ -190,37 +168,31 @@ typedef enum {
STEP_UPDATE_FILTERS,
STEP_HANNING,
STEP_COUNT
} UpdateStep_e;
};
/*
* 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);
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
}
fftSum += cubedData;
fftWeightedSum += cubedData * (i + 1); // calculate weighted index starting at 1, not 0
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;
// 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(&notchFilterDyn[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(&notchFilterDyn[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

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

@ -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);