Merge pull request #6428 from DieHertz/bfdev-gyro-analyse-refactor
Dynamic Filter refactor
This commit is contained in:
Michael Keller
GitHub
commit
cacfebe869
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <stdint.h>
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue