diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 9155f4e0a5..506d018609 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -373,7 +373,7 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = { #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_GYRO_DATA_ANALYSE -static const char * const lookupTableDynamicFilterLocation[] = { +static const char * const lookupTableDynamicFftLocation[] = { "BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS" }; #endif // USE_GYRO_DATA_ANALYSE @@ -468,7 +468,7 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType), #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_GYRO_DATA_ANALYSE - LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterLocation), + LOOKUP_TABLE_ENTRY(lookupTableDynamicFftLocation), #endif // USE_GYRO_DATA_ANALYSE }; @@ -516,12 +516,12 @@ 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_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_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_type) }, { "dyn_filter_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) }, - { "dyn_notch_quality", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) }, - { "dyn_filter_location", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_location) }, { "dyn_filter_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_threshold) }, { "dyn_filter_ignore", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_ignore) }, + { "dyn_notch_quality", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) }, #endif // PG_ACCELEROMETER_CONFIG diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index 8acd70bd94..f65435ac39 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -113,7 +113,7 @@ typedef enum { TABLE_RC_SMOOTHING_DERIVATIVE_TYPE, #endif // USE_RC_SMOOTHING_FILTER #ifdef USE_GYRO_DATA_ANALYSE - TABLE_DYNAMIC_FILTER_LOCATION, + TABLE_DYNAMIC_FFT_LOCATION, #endif // USE_GYRO_DATA_ANALYSE LOOKUP_TABLE_COUNT diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a4a2971df7..e44256e7e8 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -210,7 +210,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .dyn_filter_type = FILTER_BIQUAD, .dyn_filter_width_percent = 40, .dyn_notch_quality = 20, - .dyn_filter_location = DYN_FILTER_BEFORE_STATIC_FILTERS, + .dyn_fft_location = DYN_FFT_BEFORE_STATIC_FILTERS, .dyn_filter_threshold = 30, .dyn_filter_ignore = 20, ); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index e6d57d0472..9f12dac1b8 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -59,8 +59,8 @@ typedef enum { } gyroOverflowCheck_e; enum { - DYN_FILTER_BEFORE_STATIC_FILTERS = 0, - DYN_FILTER_AFTER_STATIC_FILTERS + DYN_FFT_BEFORE_STATIC_FILTERS = 0, + DYN_FFT_AFTER_STATIC_FILTERS } ; #define GYRO_CONFIG_USE_GYRO_1 0 @@ -104,7 +104,7 @@ typedef struct gyroConfig_s { uint8_t dyn_filter_type; uint8_t dyn_filter_width_percent; uint8_t dyn_notch_quality; // bandpass quality factor, 100 for steep sided bandpass - uint8_t dyn_filter_location; // before or after static filters + uint8_t dyn_fft_location; // before or after static filters uint8_t dyn_filter_threshold; // divided by 10 then difference needed to detect peak uint8_t dyn_filter_ignore; // ignore any FFT bin below this threshold } gyroConfig_t; diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index e79ae2f406..fdbc8cd26e 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -10,18 +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 in 3 - } - if (gyroConfig()->dyn_filter_location == DYN_FILTER_BEFORE_STATIC_FILTERS) { - gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf); - gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf); - if (axis == X) { - GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch - GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); // store post-notch data in 2 - } + 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 @@ -32,14 +26,17 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { - if (gyroConfig()->dyn_filter_location == DYN_FILTER_AFTER_STATIC_FILTERS){ - if (axis == X) { - GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); // store post-static data in debug 2 - GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after statics - } - gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf); - gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf); + 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, gyroDataForAnalysis); + gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf); } #endif