From 450cee3e082334126550552338744558ab2de010 Mon Sep 17 00:00:00 2001 From: KarateBrot Date: Tue, 9 Nov 2021 01:26:46 +0100 Subject: [PATCH] Removed dynamic notch from features --- src/main/cli/cli.c | 2 +- src/main/cli/settings.c | 2 +- src/main/cms/cms_menu_imu.c | 6 +++--- src/main/config/config.c | 14 -------------- src/main/config/feature.c | 4 ++-- src/main/config/feature.h | 2 +- src/main/flight/dyn_notch_filter.c | 20 ++++++++++++++------ src/main/flight/dyn_notch_filter.h | 6 +++--- src/main/osd/osd.c | 2 +- src/main/sensors/gyro.c | 2 +- src/main/sensors/gyro_filter_impl.c | 2 +- src/main/target/ALIENWHOOP/config.c | 2 +- 12 files changed, 29 insertions(+), 35 deletions(-) diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 41a174d15..64bd31fb0 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -265,7 +265,7 @@ static const char * const featureNames[] = { "RANGEFINDER", "TELEMETRY", "", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD", "", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", - "", "", "RX_SPI", "", "ESC_SENSOR", "ANTI_GRAVITY", "DYNAMIC_FILTER", NULL + "", "", "RX_SPI", "", "ESC_SENSOR", "ANTI_GRAVITY", "", NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 1e0592ca3..abff3f63a 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -662,7 +662,7 @@ 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) }, #endif #if defined(USE_DYN_NOTCH_FILTER) - { "dyn_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, DYN_NOTCH_COUNT_MAX }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_count) }, + { "dyn_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, DYN_NOTCH_COUNT_MAX }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_count) }, { "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_q) }, { "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 60, 250 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_min_hz) }, { "dyn_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 1000 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_max_hz) }, diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index eaec55065..fbaa2ad5b 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -827,10 +827,10 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] = { "-- DYN FILT --", OME_Label, NULL, NULL, 0 }, #ifdef USE_DYN_NOTCH_FILTER - { "NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltNotchCount, 1, DYN_NOTCH_COUNT_MAX, 1 }, 0 }, + { "NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltNotchCount, 0, DYN_NOTCH_COUNT_MAX, 1 }, 0 }, { "NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ, 1, 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 }, + { "NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz, 60, 250, 1 }, 0 }, + { "NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz, 200, 1000, 1 }, 0 }, #endif #ifdef USE_DYN_LPF diff --git a/src/main/config/config.c b/src/main/config/config.c index 4ffe3325f..1834d88b0 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -102,8 +102,6 @@ pidProfile_t *currentPidProfile; #define RX_SPI_DEFAULT_PROTOCOL 0 #endif -#define DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME HZ_TO_INTERVAL_US(2000) - PG_REGISTER_WITH_RESET_TEMPLATE(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 1); PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig, @@ -466,10 +464,6 @@ static void validateAndFixConfig(void) featureDisableImmediate(FEATURE_ESC_SENSOR); #endif -#ifndef USE_DYN_NOTCH_FILTER - featureDisableImmediate(FEATURE_DYNAMIC_FILTER); -#endif - #if !defined(USE_ADC) featureDisableImmediate(FEATURE_RSSI_ADC); #endif @@ -682,14 +676,6 @@ void validateAndFixGyroConfig(void) } } -#ifdef USE_DYN_NOTCH_FILTER - // Disable dynamic filter if gyro loop is less than 2KHz - const uint32_t configuredLooptime = (gyro.sampleRateHz > 0) ? (pidConfig()->pid_process_denom * 1e6 / gyro.sampleRateHz) : 0; - if (configuredLooptime > DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME) { - featureDisableImmediate(FEATURE_DYNAMIC_FILTER); - } -#endif - #ifdef USE_BLACKBOX #ifndef USE_FLASHFS if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) { diff --git a/src/main/config/feature.c b/src/main/config/feature.c index 5b988bbb4..27ceb10b5 100644 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -29,10 +29,10 @@ #include "pg/pg_ids.h" -PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 1); PG_RESET_TEMPLATE(featureConfig_t, featureConfig, - .enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_DYNAMIC_FILTER | FEATURE_ANTI_GRAVITY | FEATURE_AIRMODE, + .enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_ANTI_GRAVITY | FEATURE_AIRMODE, ); static uint32_t runtimeFeatureMask; diff --git a/src/main/config/feature.h b/src/main/config/feature.h index b62533d5e..240346c5a 100644 --- a/src/main/config/feature.h +++ b/src/main/config/feature.h @@ -53,7 +53,7 @@ typedef enum { //FEATURE_SOFTSPI = 1 << 26, (removed) FEATURE_ESC_SENSOR = 1 << 27, FEATURE_ANTI_GRAVITY = 1 << 28, - FEATURE_DYNAMIC_FILTER = 1 << 29, + //FEATURE_DYNAMIC_FILTER = 1 << 29, (removed) } features_e; typedef struct featureConfig_s { diff --git a/src/main/flight/dyn_notch_filter.c b/src/main/flight/dyn_notch_filter.c index af9c42282..3ecd10bb1 100644 --- a/src/main/flight/dyn_notch_filter.c +++ b/src/main/flight/dyn_notch_filter.c @@ -29,6 +29,7 @@ */ #include +#include #include "platform.h" @@ -85,6 +86,7 @@ #define DYN_NOTCH_SMOOTH_HZ 4 #define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * STEP_COUNT) // 3 axes and 4 steps per axis #define DYN_NOTCH_OSD_MIN_THROTTLE 20 +#define DYN_NOTCH_UPDATE_MIN_HZ 2000 typedef enum { @@ -119,7 +121,7 @@ typedef struct dynNotch_s { float maxHz; int count; - uint16_t maxCenterFreq; + int maxCenterFreq; float centerFreq[XYZ_AXIS_COUNT][DYN_NOTCH_COUNT_MAX]; timeUs_t looptimeUs; @@ -154,7 +156,7 @@ static FAST_DATA_ZERO_INIT float gain; void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeUs) { - // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later + // always initialise, since the dynamic notch could be activated at any time dynNotch.q = config->dyn_notch_q / 100.0f; dynNotch.minHz = config->dyn_notch_min_hz; dynNotch.maxHz = MAX(2 * dynNotch.minHz, config->dyn_notch_max_hz); @@ -164,6 +166,12 @@ void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeU // dynNotchUpdate() is running at looprateHz (which is PID looprate aka. 1e6f / gyro.targetLooptime) const float looprateHz = 1.0f / dynNotch.looptimeUs * 1e6f; + + // Disable dynamic notch if dynNotchUpdate() would run at less than 2kHz + if (looprateHz < DYN_NOTCH_UPDATE_MIN_HZ) { + dynNotch.count = 0; + } + sampleCount = MAX(1, looprateHz / (2 * dynNotch.maxHz)); // 600hz, 8k looptime, 6.00 sampleCountRcp = 1.0f / sampleCount; @@ -380,19 +388,19 @@ static FAST_CODE_NOINLINE void dynNotchProcess(void) FAST_CODE float dynNotchFilter(const int axis, float value) { - for (uint8_t p = 0; p < dynNotch.count; p++) { + for (int p = 0; p < dynNotch.count; p++) { value = biquadFilterApplyDF1(&dynNotch.notch[axis][p], value); } return value; } -bool isDynamicFilterActive(void) +bool isDynNotchActive(void) { - return featureIsEnabled(FEATURE_DYNAMIC_FILTER); + return dynNotch.count > 0; } -uint16_t getMaxFFT(void) +int getMaxFFT(void) { return dynNotch.maxCenterFreq; } diff --git a/src/main/flight/dyn_notch_filter.h b/src/main/flight/dyn_notch_filter.h index 7cc75a8f8..3789aeea6 100644 --- a/src/main/flight/dyn_notch_filter.h +++ b/src/main/flight/dyn_notch_filter.h @@ -20,7 +20,7 @@ #pragma once -#include +#include #include "common/time.h" @@ -32,6 +32,6 @@ void dynNotchInit(const dynNotchConfig_t *config, const timeUs_t targetLooptimeU void dynNotchPush(const int axis, const float sample); void dynNotchUpdate(void); float dynNotchFilter(const int axis, float value); -bool isDynamicFilterActive(void); -uint16_t getMaxFFT(void); +bool isDynNotchActive(void); +int getMaxFFT(void); void resetMaxFFT(void); diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index c620d2103..df0fa3dcc 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -827,7 +827,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow) #if defined(USE_DYN_NOTCH_FILTER) case OSD_STAT_MAX_FFT: - if (featureIsEnabled(FEATURE_DYNAMIC_FILTER)) { + if (isDynNotchActive()) { int value = getMaxFFT(); if (value > 0) { tfp_sprintf(buff, "%dHZ", value); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5b8582e6f..28af11f61 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -473,7 +473,7 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs) } #ifdef USE_DYN_NOTCH_FILTER - if (isDynamicFilterActive()) { + if (isDynNotchActive()) { dynNotchUpdate(); } #endif diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c index 676a0561b..58d7006ba 100644 --- a/src/main/sensors/gyro_filter_impl.c +++ b/src/main/sensors/gyro_filter_impl.c @@ -65,7 +65,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void) GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 3, lrintf(gyroADCf)); #ifdef USE_DYN_NOTCH_FILTER - if (isDynamicFilterActive()) { + if (isDynNotchActive()) { if (axis == gyro.gyroDebugAxis) { GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); diff --git a/src/main/target/ALIENWHOOP/config.c b/src/main/target/ALIENWHOOP/config.c index 53077257e..a85553d54 100644 --- a/src/main/target/ALIENWHOOP/config.c +++ b/src/main/target/ALIENWHOOP/config.c @@ -103,7 +103,7 @@ void targetConfiguration(void) pidConfigMutable()->runaway_takeoff_prevention = false; - featureConfigSet((FEATURE_DYNAMIC_FILTER | FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM); + featureConfigSet((FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM); /* AlienWhoop PIDs tested with 6mm and 7mm motors on most frames */ for (uint8_t pidProfileIndex = 0; pidProfileIndex < PID_PROFILE_COUNT; pidProfileIndex++) {