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

Merge pull request #6517 from ctzsnooze/DynFiltV5 

Dynamic Filter Update
This commit is contained in:
Michael Keller 2018-08-20 23:23:50 +12:00 committed by GitHub
parent a365ecea17 47e4d3d68d
commit 43e6ea31da
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 165 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/main/interface/settings.c
View File

@ -372,6 +372,15 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = {
};
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE
static const char * const lookupTableDynamicFftLocation[] = {
"BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS"
};
static const char * const lookupTableDynamicFilterRange[] = {
"HIGH", "MEDIUM", "LOW"
};
#endif // USE_GYRO_DATA_ANALYSE
#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
const lookupTableEntry_t lookupTables[] = {
@ -461,6 +470,11 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE
LOOKUP_TABLE_ENTRY(lookupTableDynamicFftLocation),
LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterRange),
#endif // USE_GYRO_DATA_ANALYSE
};
#undef LOOKUP_TABLE_ENTRY
@ -506,8 +520,9 @@ const clivalue_t valueTable[] = {
{ "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
#endif
#if defined(USE_GYRO_DATA_ANALYSE)
{ "dyn_notch_quality", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) },
{ "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) },
{ "dyn_fft_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FFT_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_fft_location) },
{ "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) },
{ "dyn_filter_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) },
#endif
// PG_ACCELEROMETER_CONFIG

5
src/main/interface/settings.h
View File

@ -112,6 +112,11 @@ typedef enum {
TABLE_RC_SMOOTHING_INPUT_TYPE,
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE
TABLE_DYNAMIC_FFT_LOCATION,
TABLE_DYNAMIC_FILTER_RANGE,
#endif // USE_GYRO_DATA_ANALYSE
LOOKUP_TABLE_COUNT
} lookupTableIndex_e;

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

@ -144,8 +144,11 @@ typedef struct gyroSensor_s {
#endif // USE_YAW_SPIN_RECOVERY
#ifdef USE_GYRO_DATA_ANALYSE
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
gyroAnalyseState_t gyroAnalyseState;
#endif
} gyroSensor_t;
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
@ -203,8 +206,9 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_offset_yaw = 0,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
.dyn_notch_quality = 70,
.dyn_notch_width_percent = 50,
.dyn_filter_width_percent = 40,
.dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS,
.dyn_filter_range = DYN_FILTER_RANGE_MEDIUM,
);
@ -510,6 +514,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
#ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime);
#endif
return true;
@ -662,7 +667,7 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint
// Dereference the pointer to null before checking valid cutoff and filter
// type. It will be overridden for positive cases.
*lowpassFilterApplyFn = &nullFilterApply;
*lowpassFilterApplyFn = nullFilterApply;
// If lowpass cutoff has been specified and is less than the Nyquist frequency
if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
@ -748,9 +753,9 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
if (isDynamicFilterActive()) {
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
const float notchQ = filterGetNotchQ(400, 390); //just any init value
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
}
}
}
@ -1013,7 +1018,7 @@ static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t current
#endif // USE_YAW_SPIN_RECOVERY
#define GYRO_FILTER_FUNCTION_NAME filterGyro
#define GYRO_FILTER_DEBUG_SET(...)
#define GYRO_FILTER_DEBUG_SET(mode, index, value) { UNUSED(mode); UNUSED(index); UNUSED(value); }
#include "gyro_filter_impl.h"
#undef GYRO_FILTER_FUNCTION_NAME
#undef GYRO_FILTER_DEBUG_SET

22
src/main/sensors/gyro.h
View File

@ -21,11 +21,14 @@
#pragma once
#include "common/axis.h"
#include "common/filter.h"
#include "common/time.h"
#include "pg/pg.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "pg/pg.h"
typedef enum {
GYRO_NONE = 0,
GYRO_DEFAULT,
@ -58,6 +61,17 @@ typedef enum {
GYRO_OVERFLOW_CHECK_ALL_AXES
} gyroOverflowCheck_e;
enum {
DYN_FFT_BEFORE_STATIC_FILTERS = 0,
DYN_FFT_AFTER_STATIC_FILTERS
} ;
enum {
DYN_FILTER_RANGE_HIGH = 0,
DYN_FILTER_RANGE_MEDIUM,
DYN_FILTER_RANGE_LOW
} ;
#define GYRO_CONFIG_USE_GYRO_1 0
#define GYRO_CONFIG_USE_GYRO_2 1
#define GYRO_CONFIG_USE_GYRO_BOTH 2
@ -96,8 +110,10 @@ typedef struct gyroConfig_s {
int16_t yaw_spin_threshold;
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
uint8_t dyn_notch_quality; // bandpass quality factor, 100 for steep sided bandpass
uint8_t dyn_notch_width_percent;
uint8_t dyn_filter_width_percent;
uint8_t dyn_fft_location; // before or after static filters
uint8_t dyn_filter_range; // ignore any FFT bin below this threshold
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);

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

@ -10,10 +10,12 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
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
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
}
}
float gyroDataForAnalysis = gyroADCf;
#endif
// apply static notch filters and software lowpass filters
@ -24,11 +26,17 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
if (gyroConfig()->dyn_fft_location == DYN_FFT_AFTER_STATIC_FILTERS) {
gyroDataForAnalysis = gyroADCf;
}
if (axis == X) {
GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroDataForAnalysis));
GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis));
}
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
}
}
#endif

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

@ -42,37 +42,35 @@
#include "sensors/gyroanalyse.h"
// The FFT splits the frequency domain into an number of bins
// 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 31.25Hz wide
// Eg [0,31), [31,62), [62, 93) etc
// for gyro loop >= 4KHz, sample rate 2000 defines FFT range to 1000Hz, 16 bins each 62.5 Hz wide
// NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
// for gyro loop >= 4KHz, analyse up to 666Hz, 16 bins each 41.625 Hz wide
#define FFT_SAMPLING_RATE_HZ 1333
// following bin must be at least 2 times previous to indicate start of peak
#define FFT_MIN_BIN_RISE 2
// the desired approimate lower frequency when calculating bin offset
#define FFT_BIN_OFFSET_DESIRED_HZ 90
// 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
// lowest allowed notch cutoff frequency
#define DYN_NOTCH_MIN_CUTOFF_HZ 105
// start to compare 3rd bin to 2nd bin, ie start comparing from 77Hz, 100Hz, or 150Hz centres
#define FFT_BIN_OFFSET 2
// smoothing frequency for FFT centre frequency
#define DYN_NOTCH_SMOOTH_FREQ_HZ 50
// notch centre point will not go below sample rate divided by these dividers, resulting in range limits:
// HIGH : 133/166-1000Hz, MEDIUM -> 89/111-666Hz, LOW -> 67/83-500Hz
#define DYN_NOTCH_MIN_CENTRE_DIV 12
// divider to get lowest allowed notch cutoff frequency
// otherwise cutoff is user configured percentage below centre frequency
#define DYN_NOTCH_MIN_CUTOFF_DIV 15
// we need 4 steps for each axis
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
// centre frequency of bandpass that constrains input to FFT
static uint16_t FAST_RAM_ZERO_INIT fftBpfHz;
// Hz per bin
static float FAST_RAM_ZERO_INIT fftResolution;
// maximum notch centre frequency limited by Nyquist
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCentreHz;
static uint8_t FAST_RAM_ZERO_INIT fftBinOffset;
static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
static float FAST_RAM_ZERO_INIT fftResolution;
static uint8_t FAST_RAM_ZERO_INIT fftBinOffset;
static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCenterHz;
static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMaxCenterHz;
static uint16_t FAST_RAM_ZERO_INIT dynamicNotchMinCutoffHz;
static float FAST_RAM_ZERO_INIT dynamicFilterWidthFactor;
static uint8_t dynamicFilterRange;
// 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 gyroDataAnalyseInit(uint32_t targetLooptimeUs)
{
@ -84,31 +82,39 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
gyroAnalyseInitialized = true;
#endif
const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
dynamicFilterRange = gyroConfig()->dyn_filter_range;
fftSamplingRateHz = 1000;
if (dynamicFilterRange == DYN_FILTER_RANGE_HIGH) {
fftSamplingRateHz = 2000;
}
else if (dynamicFilterRange == DYN_FILTER_RANGE_MEDIUM) {
fftSamplingRateHz = 1333;
}
// If we get at least 3 samples then use the default FFT sample frequency
// otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
fftSamplingRateHz = MIN((gyroLoopRateHz / 3), FFT_SAMPLING_RATE_HZ);
const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz);
fftBpfHz = fftSamplingRateHz / 4;
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
dynNotchMaxCentreHz = fftSamplingRateHz / 2;
fftBinOffset = FFT_BIN_OFFSET;
dynamicNotchMaxCenterHz = fftSamplingRateHz / 2; //Nyquist
dynamicNotchMinCenterHz = fftSamplingRateHz / DYN_NOTCH_MIN_CENTRE_DIV;
dynamicNotchMinCutoffHz = fftSamplingRateHz / DYN_NOTCH_MIN_CUTOFF_DIV;
dynamicFilterWidthFactor = (100.0f - gyroConfig()->dyn_filter_width_percent) / 100;
// Calculate the FFT bin offset to try and get the lowest bin used
// in the center calc close to 90hz
// > 1333hz = 1, 889hz (2.67K) = 2, 666hz (2K) = 3
fftBinOffset = MAX(1, lrintf(FFT_BIN_OFFSET_DESIRED_HZ / fftResolution - 1.5f));
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
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 gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
{
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
// *** can this next line be removed ??? ***
gyroDataAnalyseInit(targetLooptimeUs);
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
@ -117,14 +123,14 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
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
// 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 / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// any init value
state->centerFreq[axis] = 200;
biquadFilterInit(&state->gyroBandpassFilter[axis], fftBpfHz, 1000000 / fftSamplingRateHz, 0.01f * gyroConfig()->dyn_notch_quality, FILTER_BPF);
state->centerFreq[axis] = dynamicNotchMaxCenterHz;
state->prevCenterFreq[axis] = dynamicNotchMaxCenterHz;
biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
}
}
@ -152,8 +158,6 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
// calculate mean value of accumulated samples
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
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));
@ -251,53 +255,72 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
}
case STEP_CALC_FREQUENCIES:
{
// 13us
// calculate FFT centreFreq
float fftSum = 0;
float fftWeightedSum = 0;
bool fftIncreasing = false;
// iterate over fft data and calculate weighted indices
bool fftIncreased = false;
float dataMax = 0;
uint8_t binStart = 0;
uint8_t binMax = 0;
//for bins after initial decline, identify start bin and max bin
for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) {
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;
if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) {
if (!fftIncreased) {
binStart = i; // first up-step bin
fftIncreased = true;
}
if (state->fftData[i] > dataMax) {
dataMax = state->fftData[i];
binMax = i; // tallest bin
}
fftSum += cubedData;
// calculate weighted index starting at 1, not 0
fftWeightedSum += cubedData * (i + 1);
}
}
// accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak
float cubedData = state->fftData[binMax] * state->fftData[binMax] * state->fftData[binMax];
float fftSum = cubedData;
float fftWeightedSum = cubedData * (binMax + 1);
// accumulate upper shoulder
for (int i = binMax; i < FFT_BIN_COUNT - 1; i++) {
if (state->fftData[i] > state->fftData[i + 1]) {
cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i];
fftSum += cubedData;
fftWeightedSum += cubedData * (i + 1);
} else {
break;
}
}
// accumulate lower shoulder
for (int i = binMax; i > binStart + 1; i--) {
if (state->fftData[i] > state->fftData[i - 1]) {
cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i];
fftSum += cubedData;
fftWeightedSum += cubedData * (i + 1);
} else {
break;
}
}
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
// if no peak, go to highest point to minimise delay
float centerFreq = dynNotchMaxCentreHz;
float centerFreq = dynamicNotchMaxCenterHz;
float fftMeanIndex = 0;
// idx was shifted by 1 to start at 1, not 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 * fftResolution, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz);
centerFreq = fftMeanIndex * fftResolution;
} else {
centerFreq = state->prevCenterFreq[state->updateAxis];
}
// constrain and low-pass smooth centre frequency
centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz);
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz);
centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz);
state->centerFreq[state->updateAxis] = centerFreq;
if (state->updateAxis == 0) {
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
}
DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis, state->centerFreq[state->updateAxis]);
if (state->updateAxis == 1) {
DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
}
// Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break;
}
@ -305,9 +328,10 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
{
// 7us
// calculate cutoffFreq and notch Q, update notch filter
const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicNotchCutoff, DYN_NOTCH_MIN_CUTOFF_HZ);
const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor, dynamicNotchMinCutoffHz);
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);
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;

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

@ -22,9 +22,10 @@
#include "arm_math.h"
#include "common/time.h"
#include "common/filter.h"
#include "sensors/gyro.h"
// max for F3 targets
#define FFT_WINDOW_SIZE 32
@ -35,9 +36,6 @@ typedef struct gyroAnalyseState_s {
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];
@ -53,6 +51,7 @@ typedef struct gyroAnalyseState_s {
biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
uint16_t centerFreq[XYZ_AXIS_COUNT];
uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
} gyroAnalyseState_t;
STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type);