Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 07:15:18 +03:00
Code Issues Wiki Activity

Simplify and improve Dynamic Notch

Browse source 
Remove prevCenterFreq[axis]
Simpler peak detection:
- cubes, not squares
- only look one bin either side
More accurate frequency determination
Updated annotations and explanations
Whitespace fixes
fftStartBin can't be less than 1
Dynamic PT1 smoothing
biquad filter on centerFreq removed
centerFreq's now floats

Limits and better min max logic

constrain within nyquist limits

If gyro rate was 2khz and dynamic notch range max, this will keep the notch max to 960hz.

Simple fixes

Get lowest min above and below the peak

First go at setting DN range by a maxHz

increment the PG version in gyro.c

MSP correct size

Resolve comments from Mike and Rav.

Fis msp, deprecating range
This commit is contained in:
ctzsnooze 2019-12-31 12:28:38 +11:00 • committed by mikeller
parent 6497830a1b
commit 7b2e075b3a
9 changed files with 165 additions and 161 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View file

@ -1373,7 +1373,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
gyroConfig()->gyro_soft_notch_cutoff_2); gyroConfig()->gyro_soft_notch_cutoff_2);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_range", "%d", gyroConfig()->dyn_notch_range); BLACKBOX_PRINT_HEADER_LINE("dyn_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_width_percent", "%d", gyroConfig()->dyn_notch_width_percent); BLACKBOX_PRINT_HEADER_LINE("dyn_notch_width_percent", "%d", gyroConfig()->dyn_notch_width_percent);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q); BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz); BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);

13
src/main/cli/settings.c
View file

@ -408,12 +408,6 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = {
}; };
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE
static const char * const lookupTableDynamicFilterRange[] = {
"HIGH", "MEDIUM", "LOW", "AUTO"
};
#endif // USE_GYRO_DATA_ANALYSE
#ifdef USE_VTX_COMMON #ifdef USE_VTX_COMMON
static const char * const lookupTableVtxLowPowerDisarm[] = { static const char * const lookupTableVtxLowPowerDisarm[] = {
"OFF", "ON", "UNTIL_FIRST_ARM" "OFF", "ON", "UNTIL_FIRST_ARM"
@ -588,9 +582,6 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType), LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType),
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType), LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE
LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterRange),
#endif // USE_GYRO_DATA_ANALYSE
#ifdef USE_VTX_COMMON #ifdef USE_VTX_COMMON
LOOKUP_TABLE_ENTRY(lookupTableVtxLowPowerDisarm), LOOKUP_TABLE_ENTRY(lookupTableVtxLowPowerDisarm),
#endif #endif
@ -658,10 +649,10 @@ const clivalue_t valueTable[] = {
{ "gyro_to_use", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, { "gyro_to_use", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
#endif #endif
#if defined(USE_GYRO_DATA_ANALYSE) #if defined(USE_GYRO_DATA_ANALYSE)
{ "dyn_notch_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_range) }, { "dyn_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_max_hz) },
{ "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) }, { "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) },
{ "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) }, { "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) },
{ "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 60, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) }, { "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 60, 250 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
#endif #endif
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
{ "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) }, { "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) },

3
src/main/cli/settings.h
View file

@ -114,9 +114,6 @@ typedef enum {
TABLE_RC_SMOOTHING_INPUT_TYPE, TABLE_RC_SMOOTHING_INPUT_TYPE,
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE, TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_GYRO_DATA_ANALYSE
TABLE_DYNAMIC_FILTER_RANGE,
#endif // USE_GYRO_DATA_ANALYSE
#ifdef USE_VTX_COMMON #ifdef USE_VTX_COMMON
TABLE_VTX_LOW_POWER_DISARM, TABLE_VTX_LOW_POWER_DISARM,
#endif #endif

14
src/main/cms/cms_menu_imu.c
View file

@ -77,12 +77,6 @@ static const char * const osdTableThrottleLimitType[] = {
"OFF", "SCALE", "CLIP" "OFF", "SCALE", "CLIP"
}; };
#if defined(USE_GYRO_DATA_ANALYSE) && defined(USE_EXTENDED_CMS_MENUS)
static const char * const osdTableDynNotchRangeType[] = {
"HIGH", "MED", "LOW", "AUTO"
};
#endif
#ifdef USE_MULTI_GYRO #ifdef USE_MULTI_GYRO
static const char * const osdTableGyroToUse[] = { static const char * const osdTableGyroToUse[] = {
"FIRST", "SECOND", "BOTH" "FIRST", "SECOND", "BOTH"
@ -570,7 +564,7 @@ static CMS_Menu cmsx_menuFilterGlobal = {
#if (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS) #if (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
static uint8_t dynFiltNotchRange; static uint16_t dynFiltNotchMaxHz;
static uint8_t dynFiltWidthPercent; static uint8_t dynFiltWidthPercent;
static uint16_t dynFiltNotchQ; static uint16_t dynFiltNotchQ;
static uint16_t dynFiltNotchMinHz; static uint16_t dynFiltNotchMinHz;
@ -586,7 +580,7 @@ static uint8_t dynFiltDtermExpo;
static const void *cmsx_menuDynFilt_onEnter(void) static const void *cmsx_menuDynFilt_onEnter(void)
{ {
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
dynFiltNotchRange = gyroConfig()->dyn_notch_range; dynFiltNotchMaxHz = gyroConfig()->dyn_notch_max_hz;
dynFiltWidthPercent = gyroConfig()->dyn_notch_width_percent; dynFiltWidthPercent = gyroConfig()->dyn_notch_width_percent;
dynFiltNotchQ = gyroConfig()->dyn_notch_q; dynFiltNotchQ = gyroConfig()->dyn_notch_q;
dynFiltNotchMinHz = gyroConfig()->dyn_notch_min_hz; dynFiltNotchMinHz = gyroConfig()->dyn_notch_min_hz;
@ -608,7 +602,7 @@ static const void *cmsx_menuDynFilt_onExit(const OSD_Entry *self)
UNUSED(self); UNUSED(self);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
gyroConfigMutable()->dyn_notch_range = dynFiltNotchRange; gyroConfigMutable()->dyn_notch_max_hz = dynFiltNotchMaxHz;
gyroConfigMutable()->dyn_notch_width_percent = dynFiltWidthPercent; gyroConfigMutable()->dyn_notch_width_percent = dynFiltWidthPercent;
gyroConfigMutable()->dyn_notch_q = dynFiltNotchQ; gyroConfigMutable()->dyn_notch_q = dynFiltNotchQ;
gyroConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz; gyroConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz;
@ -630,10 +624,10 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] =
{ "-- DYN FILT --", OME_Label, NULL, NULL, 0 }, { "-- DYN FILT --", OME_Label, NULL, NULL, 0 },
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
{ "NOTCH RANGE", OME_TAB, NULL, &(OSD_TAB_t) { &dynFiltNotchRange, 3, osdTableDynNotchRangeType}, 0 },
{ "NOTCH WIDTH %", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltWidthPercent, 0, 20, 1 }, 0 }, { "NOTCH WIDTH %", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltWidthPercent, 0, 20, 1 }, 0 },
{ "NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ, 0, 1000, 1 }, 0 }, { "NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ, 0, 1000, 1 }, 0 },
{ "NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz, 0, 1000, 1 }, 0 }, { "NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz, 0, 1000, 1 }, 0 },
{ "NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz, 0, 1000, 1 }, 0 },
#endif #endif
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF

245
src/main/flight/gyroanalyse.c
View file

@ -44,31 +44,60 @@
#include "gyroanalyse.h" #include "gyroanalyse.h"
// The FFT splits the frequency domain into an number of bins // FFT_WINDOW_SIZE defaults to 32 (gyroanalyse.h)
// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each 31.25Hz wide // We get 16 frequency bins from 32 consecutive data values
// Eg [0,31), [31,62), [62, 93) etc // Bin 0 is DC and can't be used.
// for gyro loop >= 4KHz, sample rate 2000 defines FFT range to 1000Hz, 16 bins each 62.5 Hz wide // Only bins 1 to 15 are usable.
// NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
// smoothing frequency for FFT centre frequency
#define DYN_NOTCH_SMOOTH_FREQ_HZ 50
// we need 4 steps for each axis
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
// A gyro sample is collected every gyro loop
// maxSampleCount recent gyro values are accumulated and averaged
// to ensure that 32 samples are collected at the right rate for the required FFT bandwidth
// For an 8k gyro loop, at default 600hz max, 6 sequential gyro data points are averaged, FFT runs 1333Hz.
// Upper limit of FFT is half that frequency, eg 666Hz by default.
// At 8k, if user sets a max of 300Hz, int(8000/600) = 13, fftSamplingRateHz = 615Hz, range 307Hz
// Note that lower max requires more samples to be averaged, increasing precision but taking longer to get enough samples.
// For Bosch at 3200Hz gyro, max of 600, int(3200/1200) = 2, fftSamplingRateHz = 1600, range to 800hz
// For Bosch on XClass, better to set a max of 300, int(3200/600) = 5, fftSamplingRateHz = 640, range to 320Hz
//
// When sampleCount reaches maxSampleCount, the averaged gyro value is put into the circular buffer of 32 samples
// At 8k, with 600Hz max, maxSampleCount = 6, this happens every 6 * 0.125us, or every 0.75ms
// Hence to completely replace all 32 samples of the FFT input buffer with clean new data takes 24ms
// The FFT code is split into steps. It takes 4 gyro loops to calculate the FFT for one axis
// (gyroDataAnalyseUpdate has 8 steps, but only four breaks)
// Since there are three axes, it takes 12 gyro loops to completely update all axes.
// At 8k, any one axis gets updated at 8000 / 12 or 666hz or every 1.5ms
// In this time, 2 points in the FFT buffer will have changed.
// At 4k, it takes twice as long to update an axis, i.e. each axis updates only every 3ms
// Four points in the buffer will have changed in that time, and each point will be the average of three samples.
// Hence output jitter at 4k is about four times worse than at 8k. At 2k output jitter is quite bad.
// The Hanning window step loads gyro data (32 data points) for one axis from the circular buffer into fftData[i]
// and applies the hanning window to the edge values
// Calculation steps 1 and 2 then calculate the fft output (32 data points) and put that back into the same fftData[i] array.
// We then use fftData[i] array for frequency centre calculations for that axis
// Each FFT output bin has width fftSamplingRateHz/32, ie 41.65Hz per bin at 1333Hz
// Usable bandwidth is half this, ie 666Hz if fftSamplingRateHz is 1333Hz, i.e. bin 1 is 41.65hz, bin 2 83.3hz etc
#define DYN_NOTCH_SMOOTH_HZ 4
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) // 16
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // 4 steps per axis
#define DYN_NOTCH_OSD_MIN_THROTTLE 20 #define DYN_NOTCH_OSD_MIN_THROTTLE 20
static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz; static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
static float FAST_RAM_ZERO_INIT fftResolution; static float FAST_RAM_ZERO_INIT fftResolution;
static uint8_t FAST_RAM_ZERO_INIT fftStartBin; static uint8_t FAST_RAM_ZERO_INIT fftStartBin;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCtrHz;
static uint8_t dynamicFilterRange;
static float FAST_RAM_ZERO_INIT dynNotchQ; static float FAST_RAM_ZERO_INIT dynNotchQ;
static float FAST_RAM_ZERO_INIT dynNotch1Ctr; static float FAST_RAM_ZERO_INIT dynNotch1Ctr;
static float FAST_RAM_ZERO_INIT dynNotch2Ctr; static float FAST_RAM_ZERO_INIT dynNotch2Ctr;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMinHz; static uint16_t FAST_RAM_ZERO_INIT dynNotchMinHz;
static bool FAST_RAM dualNotch = true; static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxHz;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxFFT; static bool FAST_RAM dualNotch = true;
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxFFT;
static float FAST_RAM_ZERO_INIT smoothFactor;
static uint8_t FAST_RAM_ZERO_INIT samples;
// 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];
@ -82,43 +111,30 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
gyroAnalyseInitialized = true; gyroAnalyseInitialized = true;
#endif #endif
dynamicFilterRange = gyroConfig()->dyn_notch_range;
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f; dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f; dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f; dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz; dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
if (gyroConfig()->dyn_notch_width_percent == 0) { if (gyroConfig()->dyn_notch_width_percent == 0) {
dualNotch = false; dualNotch = false;
} }
if (dynamicFilterRange == DYN_NOTCH_RANGE_AUTO) {
if (gyroConfig()->dyn_lpf_gyro_max_hz > 333) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
}
if (gyroConfig()->dyn_lpf_gyro_max_hz > 610) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
}
} else {
if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
}
else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) {
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
}
}
// 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)
const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f); const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
samples = MAX(1, gyroLoopRateHz / (2 * dynNotchMaxHz)); //600hz, 8k looptime, 13.333
fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz);
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; fftSamplingRateHz = gyroLoopRateHz / samples;
// eg 8k, user max 600hz, int(8000/1200) = 6 (6.666), fftSamplingRateHz = 1333hz, range 666Hz
// eg 4k, user max 600hz, int(4000/1200) = 3 (3.333), fftSamplingRateHz = 1333hz, range 666Hz
// eg 2k, user max 600hz, int(2000/1200) = 1 (1.666) fftSamplingRateHz = 2000hz, range 1000Hz
// eg 2k, user max 400hz, int(2000/800) = 2 (2.5) fftSamplingRateHz = 1000hz, range 500Hz
// eg 1k, user max 600hz, int(1000/1200) = 1 (max(1,0.8333)) fftSamplingRateHz = 1000hz, range 500Hz
// the upper limit of DN is always going to be Nyquist
fftStartBin = dynNotchMinHz / lrintf(fftResolution); fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; // 41.65hz per bin for medium
fftStartBin = MAX(2, dynNotchMinHz / lrintf(fftResolution)); // can't use bin 0 because it is DC.
dynNotchMaxCtrHz = fftSamplingRateHz / 2; //Nyquist smoothFactor = 2 * M_PIf * DYN_NOTCH_SMOOTH_HZ / (gyroLoopRateHz / 12); // minimum PT1 k value
for (int i = 0; i < FFT_WINDOW_SIZE; i++) { 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))); hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
@ -128,24 +144,13 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, 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 // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
// *** can this next line be removed ??? ***
gyroDataAnalyseInit(targetLooptimeUs); gyroDataAnalyseInit(targetLooptimeUs);
state->maxSampleCount = samples;
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs; state->maxSampleCountRcp = 1.0f / state->maxSampleCount;
state->maxSampleCount = samplingFrequency / fftSamplingRateHz;
state->maxSampleCountRcp = 1.f / state->maxSampleCount;
arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE); 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 / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// any init value // any init value
state->centerFreq[axis] = dynNotchMaxCtrHz; state->centerFreq[axis] = dynNotchMaxHz;
state->prevCenterFreq[axis] = dynNotchMaxCtrHz;
biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
} }
} }
@ -162,10 +167,9 @@ static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *not
void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2) void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
{ {
// 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 and average multiple gyro samples
state->sampleCount++; state->sampleCount++;
// this runs at 1kHz
if (state->sampleCount == state->maxSampleCount) { if (state->sampleCount == state->maxSampleCount) {
state->sampleCount = 0; state->sampleCount = 0;
@ -183,6 +187,9 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn,
state->circularBufferIdx = (state->circularBufferIdx + 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 // We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value
// 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 8kHz / 4 / 3 = 666Hz => update every 1.5ms
// at 4kHz gyro loop rate this means 4kHz / 4 / 3 = 333Hz => update every 3ms
state->updateTicks = DYN_NOTCH_CALC_TICKS; state->updateTicks = DYN_NOTCH_CALC_TICKS;
} }
@ -200,7 +207,7 @@ void arm_radix8_butterfly_f32(float32_t *pSrc, uint16_t fftLen, const float32_t
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);
/* /*
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds * Analyse gyro data
*/ */
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2) static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
{ {
@ -241,6 +248,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
break; break;
} }
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break; break;
} }
case STEP_BITREVERSAL: case STEP_BITREVERSAL:
@ -257,6 +265,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
// this does not work in place => fftData AND rfftData needed // this does not work in place => fftData AND rfftData needed
stage_rfft_f32(&state->fftInstance, state->fftData, state->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:
@ -269,62 +278,76 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
} }
case STEP_CALC_FREQUENCIES: case STEP_CALC_FREQUENCIES:
{ {
bool fftIncreased = false; // identify max bin and max/min heights
float dataMax = 0; float dataMax = 0.0f;
uint8_t binStart = 0; float dataMin = 1.0f;
uint8_t binMax = 0; uint8_t binMax = 0;
//for bins after initial decline, identify start bin and max bin float dataMinHi = 1.0f;
for (int i = fftStartBin; i < FFT_BIN_COUNT; i++) { for (int i = fftStartBin; i < FFT_BIN_COUNT; i++) {
if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) { if (state->fftData[i] > state->fftData[i - 1]) { // bin height increased
if (!fftIncreased) {
binStart = i; // first up-step bin
fftIncreased = true;
}
if (state->fftData[i] > dataMax) { if (state->fftData[i] > dataMax) {
dataMax = state->fftData[i]; dataMax = state->fftData[i];
binMax = i; // tallest bin binMax = i; // tallest bin so far
} }
} }
} }
if (binMax == 0) { // no bin increase, hold prev max bin, dataMin = 1 dataMax = 0, ie move slow
binMax = lrintf(state->centerFreq[state->updateAxis] / fftResolution);
} else { // there was a max, find min
for (int i = binMax - 1; i > 1; i--) { // look for min below max
dataMin = state->fftData[i];
if (state->fftData[i - 1] > state->fftData[i]) { // up step below this one
break;
}
}
for (int i = binMax + 1; i < (FFT_BIN_COUNT - 1); i++) { // // look for min above max
dataMinHi = state->fftData[i];
if (state->fftData[i] < state->fftData[i + 1]) { // up step above this one
break;
}
}
}
dataMin = fminf(dataMin, dataMinHi);
// accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak // 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 squaredData = state->fftData[binMax] * state->fftData[binMax];
float fftSum = cubedData; float fftSum = squaredData;
float fftWeightedSum = cubedData * (binMax + 1); float fftWeightedSum = squaredData * binMax;
// accumulate upper shoulder
for (int i = binMax; i < FFT_BIN_COUNT - 1; i++) { // accumulate upper shoulder unless it would be FFT_BIN_COUNT
if (state->fftData[i] > state->fftData[i + 1]) { uint8_t shoulderBin = binMax + 1;
cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i]; if (shoulderBin < FFT_BIN_COUNT) {
fftSum += cubedData; squaredData = state->fftData[shoulderBin] * state->fftData[shoulderBin];
fftWeightedSum += cubedData * (i + 1); fftSum += squaredData;
} else { fftWeightedSum += squaredData * shoulderBin;
break;
}
} }
// accumulate lower shoulder
for (int i = binMax; i > binStart + 1; i--) { // accumulate lower shoulder unless lower shoulder would be bin 0 (DC)
if (state->fftData[i] > state->fftData[i - 1]) { if (binMax > 1) {
cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i]; shoulderBin = binMax - 1;
fftSum += cubedData; squaredData = state->fftData[shoulderBin] * state->fftData[shoulderBin];
fftWeightedSum += cubedData * (i + 1); fftSum += squaredData;
} else { fftWeightedSum += squaredData * shoulderBin;
break;
}
} }
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
float centerFreq = dynNotchMaxCtrHz; // get centerFreq in Hz from weighted bins
float centerFreq = dynNotchMaxHz;
float fftMeanIndex = 0; float fftMeanIndex = 0;
// idx was shifted by 1 to start at 1, not 0
if (fftSum > 0) { if (fftSum > 0) {
fftMeanIndex = (fftWeightedSum / fftSum) - 1; fftMeanIndex = (fftWeightedSum / fftSum);
// the index points at the center frequency of each bin so index 0 is actually 16.125Hz
centerFreq = fftMeanIndex * fftResolution; centerFreq = fftMeanIndex * fftResolution;
// In theory, the index points to the centre frequency of the bin.
// at 1333hz, bin widths are 41.65Hz, so bin 2 has the range 83,3Hz to 124,95Hz
// Rav feels that maybe centerFreq = (fftMeanIndex + 0.5) * fftResolution; is better
// empirical checking shows that not adding 0.5 works better
} else { } else {
centerFreq = state->prevCenterFreq[state->updateAxis]; centerFreq = state->centerFreq[state->updateAxis];
} }
centerFreq = fmax(centerFreq, dynNotchMinHz); centerFreq = constrainf(centerFreq, dynNotchMinHz, dynNotchMaxHz);
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis]; // PT1 style dynamic smoothing moves rapidly towards big peaks and slowly away, up to 8x faster
state->centerFreq[state->updateAxis] = centerFreq; float dynamicFactor = constrainf(dataMax / dataMin, 1.0f, 8.0f);
state->centerFreq[state->updateAxis] = state->centerFreq[state->updateAxis] + smoothFactor * dynamicFactor * (centerFreq - state->centerFreq[state->updateAxis]);
if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) { if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]); dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
@ -333,26 +356,25 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
if (state->updateAxis == 0) { 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, 0, state->centerFreq[state->updateAxis]); DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
DEBUG_SET(DEBUG_FFT_FREQ, 1, lrintf(dynamicFactor * 100));
DEBUG_SET(DEBUG_DYN_LPF, 1, state->centerFreq[state->updateAxis]); DEBUG_SET(DEBUG_DYN_LPF, 1, state->centerFreq[state->updateAxis]);
} }
if (state->updateAxis == 1) { // if (state->updateAxis == 1) {
DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]); // 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); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break; break;
} }
case STEP_UPDATE_FILTERS: case STEP_UPDATE_FILTERS:
{ {
// 7us // 7us
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004) // calculate cutoffFreq and notch Q, update notch filter
if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) { if (dualNotch) {
if (dualNotch) { biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH); biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH); } else {
} else { biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
}
} }
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
@ -363,8 +385,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
case STEP_HANNING: case STEP_HANNING:
{ {
// 5us // 5us
// apply hanning window to gyro samples and store result in fftData // apply hanning window to gyro samples and store result in fftData[i] to be used in step 1 and 2 and 3
// hanning starts and ends with 0, could be skipped for minor speed improvement
const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx; const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx;
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx); arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx);
if (state->circularBufferIdx > 0) { if (state->circularBufferIdx > 0) {

7
src/main/flight/gyroanalyse.h
View file

@ -24,8 +24,6 @@
#include "common/filter.h" #include "common/filter.h"
// max for F3 targets
#define FFT_WINDOW_SIZE 32 #define FFT_WINDOW_SIZE 32
typedef struct gyroAnalyseState_s { typedef struct gyroAnalyseState_s {
@ -48,9 +46,8 @@ typedef struct gyroAnalyseState_s {
float fftData[FFT_WINDOW_SIZE]; float fftData[FFT_WINDOW_SIZE];
float rfftData[FFT_WINDOW_SIZE]; float rfftData[FFT_WINDOW_SIZE];
biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT]; float centerFreq[XYZ_AXIS_COUNT];
uint16_t centerFreq[XYZ_AXIS_COUNT];
uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
} gyroAnalyseState_t; } gyroAnalyseState_t;
STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type); STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type);

21
src/main/msp/msp.c
View file

@ -1686,7 +1686,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
#endif #endif
// Added in MSP API 1.42 // Added in MSP API 1.42
#if defined(USE_GYRO_DATA_ANALYSE) #if defined(USE_GYRO_DATA_ANALYSE)
sbufWriteU8(dst, gyroConfig()->dyn_notch_range); sbufWriteU8(dst, 0); // DEPRECATED 1.43: dyn_notch_range
sbufWriteU8(dst, gyroConfig()->dyn_notch_width_percent); sbufWriteU8(dst, gyroConfig()->dyn_notch_width_percent);
sbufWriteU16(dst, gyroConfig()->dyn_notch_q); sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz); sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
@ -1696,7 +1696,6 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
#endif #endif
#if defined(USE_RPM_FILTER) #if defined(USE_RPM_FILTER)
sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_harmonics); sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_harmonics);
sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_min); sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_min);
@ -1704,6 +1703,12 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
#endif #endif
#if defined(USE_GYRO_DATA_ANALYSE)
// Added in MSP API 1.43
sbufWriteU16(dst, gyroConfig()->dyn_notch_max_hz);
#else
sbufWriteU16(dst, 0);
#endif
break; break;
case MSP_PID_ADVANCED: case MSP_PID_ADVANCED:
@ -2497,7 +2502,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
if (sbufBytesRemaining(src) >= 8) { if (sbufBytesRemaining(src) >= 8) {
// Added in MSP API 1.42 // Added in MSP API 1.42
#if defined(USE_GYRO_DATA_ANALYSE) #if defined(USE_GYRO_DATA_ANALYSE)
gyroConfigMutable()->dyn_notch_range = sbufReadU8(src); sbufReadU8(src); // DEPRECATED: dyn_notch_range
gyroConfigMutable()->dyn_notch_width_percent = sbufReadU8(src); gyroConfigMutable()->dyn_notch_width_percent = sbufReadU8(src);
gyroConfigMutable()->dyn_notch_q = sbufReadU16(src); gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src); gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
@ -2507,7 +2512,6 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU16(src); sbufReadU16(src);
sbufReadU16(src); sbufReadU16(src);
#endif #endif
#if defined(USE_RPM_FILTER) #if defined(USE_RPM_FILTER)
rpmFilterConfigMutable()->gyro_rpm_notch_harmonics = sbufReadU8(src); rpmFilterConfigMutable()->gyro_rpm_notch_harmonics = sbufReadU8(src);
rpmFilterConfigMutable()->gyro_rpm_notch_min = sbufReadU8(src); rpmFilterConfigMutable()->gyro_rpm_notch_min = sbufReadU8(src);
@ -2516,6 +2520,15 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU8(src); sbufReadU8(src);
#endif #endif
} }
if (sbufBytesRemaining(src) >= 1) {
#if defined(USE_GYRO_DATA_ANALYSE)
// Added in MSP API 1.43
gyroConfigMutable()->dyn_notch_max_hz = sbufReadU16(src);
#else
sbufReadU16(src);
#endif
}
// reinitialize the gyro filters with the new values // reinitialize the gyro filters with the new values
validateAndFixGyroConfig(); validateAndFixGyroConfig();

4
src/main/sensors/gyro.c
View file

@ -157,7 +157,7 @@ static void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz);
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro) #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro) #define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 7); PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8);
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
@ -193,7 +193,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->yaw_spin_threshold = 1950; gyroConfig->yaw_spin_threshold = 1950;
gyroConfig->dyn_lpf_gyro_min_hz = 200; gyroConfig->dyn_lpf_gyro_min_hz = 200;
gyroConfig->dyn_lpf_gyro_max_hz = 500; gyroConfig->dyn_lpf_gyro_max_hz = 500;
gyroConfig->dyn_notch_range = DYN_NOTCH_RANGE_MEDIUM; gyroConfig->dyn_notch_max_hz = 600;
gyroConfig->dyn_notch_width_percent = 8; gyroConfig->dyn_notch_width_percent = 8;
gyroConfig->dyn_notch_q = 120; gyroConfig->dyn_notch_q = 120;
gyroConfig->dyn_notch_min_hz = 150; gyroConfig->dyn_notch_min_hz = 150;

17
src/main/sensors/gyro.h
View file

@ -82,17 +82,6 @@ enum {
GYRO_OVERFLOW_CHECK_ALL_AXES GYRO_OVERFLOW_CHECK_ALL_AXES
}; };
enum {
DYN_NOTCH_RANGE_HIGH = 0,
DYN_NOTCH_RANGE_MEDIUM,
DYN_NOTCH_RANGE_LOW,
DYN_NOTCH_RANGE_AUTO
};
#define DYN_NOTCH_RANGE_HZ_HIGH 2000
#define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
#define DYN_NOTCH_RANGE_HZ_LOW 1000
enum { enum {
DYN_LPF_NONE = 0, DYN_LPF_NONE = 0,
DYN_LPF_PT1, DYN_LPF_PT1,
@ -144,13 +133,15 @@ typedef struct gyroConfig_s {
int16_t yaw_spin_threshold; int16_t yaw_spin_threshold;
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
uint16_t dyn_lpf_gyro_min_hz; uint16_t dyn_lpf_gyro_min_hz;
uint16_t dyn_lpf_gyro_max_hz; uint16_t dyn_lpf_gyro_max_hz;
uint8_t dyn_notch_range; // ignore any FFT bin below this threshold
uint16_t dyn_notch_max_hz;
uint8_t dyn_notch_width_percent; uint8_t dyn_notch_width_percent;
uint16_t dyn_notch_q; uint16_t dyn_notch_q;
uint16_t dyn_notch_min_hz; uint16_t dyn_notch_min_hz;
uint8_t gyro_filter_debug_axis; uint8_t gyro_filter_debug_axis;
} gyroConfig_t; } gyroConfig_t;