From 15d1df3c770374ab3698207636328aee3eec9bfa Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 6 Jul 2020 19:33:06 +1200 Subject: [PATCH] Removed DTerm RPM filter. --- src/main/blackbox/blackbox.c | 3 --- src/main/cli/settings.c | 3 --- src/main/fc/init.c | 1 - src/main/flight/pid.c | 7 ------- src/main/flight/rpm_filter.c | 27 ++++----------------------- src/main/flight/rpm_filter.h | 4 ---- 6 files changed, 4 insertions(+), 41 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 0e6e09ee8e..d2a912c119 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1430,9 +1430,6 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_harmonics", "%d", rpmFilterConfig()->gyro_rpm_notch_harmonics); BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_q", "%d", rpmFilterConfig()->gyro_rpm_notch_q); BLACKBOX_PRINT_HEADER_LINE("gyro_rpm_notch_min", "%d", rpmFilterConfig()->gyro_rpm_notch_min); - BLACKBOX_PRINT_HEADER_LINE("dterm_rpm_notch_harmonics", "%d", rpmFilterConfig()->dterm_rpm_notch_harmonics); - BLACKBOX_PRINT_HEADER_LINE("dterm_rpm_notch_q", "%d", rpmFilterConfig()->dterm_rpm_notch_q); - BLACKBOX_PRINT_HEADER_LINE("dterm_rpm_notch_min", "%d", rpmFilterConfig()->dterm_rpm_notch_min); BLACKBOX_PRINT_HEADER_LINE("rpm_notch_lpf", "%d", rpmFilterConfig()->rpm_lpf); #endif #if defined(USE_ACC) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ed29b72b97..fb80fdde56 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1617,9 +1617,6 @@ const clivalue_t valueTable[] = { { "gyro_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_harmonics) }, { "gyro_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) }, { "gyro_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_min) }, - { "dterm_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_harmonics) }, - { "dterm_rpm_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_q) }, - { "dterm_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_min) }, { "rpm_notch_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_lpf) }, #endif diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 6999b55a1f..911d0b7ce4 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -96,7 +96,6 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/pid_init.h" -#include "flight/rpm_filter.h" #include "flight/servos.h" #include "io/asyncfatfs/asyncfatfs.h" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8fbffdd53e..ec50cff56e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -47,7 +47,6 @@ #include "flight/gps_rescue.h" #include "flight/imu.h" #include "flight/mixer.h" -#include "flight/rpm_filter.h" #include "flight/interpolated_setpoint.h" #include "io/gps.h" @@ -866,18 +865,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim DEBUG_SET(DEBUG_D_LPF, 1, lrintf(delta)); } -#ifdef USE_RPM_FILTER - gyroRateDterm[axis] = rpmFilterDterm(axis,gyroRateDterm[axis]); -#endif gyroRateDterm[axis] = pidRuntime.dtermNotchApplyFn((filter_t *) &pidRuntime.dtermNotch[axis], gyroRateDterm[axis]); gyroRateDterm[axis] = pidRuntime.dtermLowpassApplyFn((filter_t *) &pidRuntime.dtermLowpass[axis], gyroRateDterm[axis]); gyroRateDterm[axis] = pidRuntime.dtermLowpass2ApplyFn((filter_t *) &pidRuntime.dtermLowpass2[axis], gyroRateDterm[axis]); } rotateItermAndAxisError(); -#ifdef USE_RPM_FILTER - rpmFilterUpdate(); -#endif #ifdef USE_INTERPOLATED_SP bool newRcFrame = false; diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 9e46162ecb..fdad9b70a6 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -72,7 +72,6 @@ FAST_RAM_ZERO_INIT static uint8_t filterUpdatesPerIteration; FAST_RAM_ZERO_INIT static float pidLooptime; FAST_RAM_ZERO_INIT static rpmNotchFilter_t filters[2]; FAST_RAM_ZERO_INIT static rpmNotchFilter_t* gyroFilter; -FAST_RAM_ZERO_INIT static rpmNotchFilter_t* dtermFilter; FAST_RAM_ZERO_INIT static uint8_t currentMotor; FAST_RAM_ZERO_INIT static uint8_t currentHarmonic; @@ -81,7 +80,7 @@ FAST_RAM static rpmNotchFilter_t* currentFilter = &filters[0]; -PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 3); +PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 4); void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config) { @@ -89,10 +88,6 @@ void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config) config->gyro_rpm_notch_min = 100; config->gyro_rpm_notch_q = 500; - config->dterm_rpm_notch_harmonics = 0; - config->dterm_rpm_notch_min = 100; - config->dterm_rpm_notch_q = 500; - config->rpm_lpf = 150; } @@ -120,7 +115,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config) numberRpmNotchFilters = 0; if (!motorConfig()->dev.useDshotTelemetry) { - gyroFilter = dtermFilter = NULL; + gyroFilter = NULL; return; } @@ -134,15 +129,6 @@ void rpmFilterInit(const rpmFilterConfig_t *config) } else { gyroFilter = NULL; } - if (config->dterm_rpm_notch_harmonics) { - dtermFilter = &filters[numberRpmNotchFilters++]; - rpmNotchFilterInit(dtermFilter, config->dterm_rpm_notch_harmonics, - config->dterm_rpm_notch_min, config->dterm_rpm_notch_q, pidLooptime); - // don't go quite to nyquist to avoid oscillations - dtermFilter->maxHz = 0.48f / (pidLooptime * 1e-6f); - } else { - dtermFilter = NULL; - } for (int i = 0; i < getMotorCount(); i++) { pt1FilterInit(&rpmFilters[i], pt1FilterGain(config->rpm_lpf, pidLooptime * 1e-6f)); @@ -174,16 +160,11 @@ float rpmFilterGyro(int axis, float value) return applyFilter(gyroFilter, axis, value); } -float rpmFilterDterm(int axis, float value) -{ - return applyFilter(dtermFilter, axis, value); -} - FAST_RAM_ZERO_INIT static float motorFrequency[MAX_SUPPORTED_MOTORS]; FAST_CODE_NOINLINE void rpmFilterUpdate() { - if (gyroFilter == NULL && dtermFilter == NULL) { + if (gyroFilter == NULL) { return; } @@ -232,7 +213,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate() bool isRpmFilterEnabled(void) { - return (motorConfig()->dev.useDshotTelemetry && (rpmFilterConfig()->gyro_rpm_notch_harmonics || rpmFilterConfig()->dterm_rpm_notch_harmonics)); + return (motorConfig()->dev.useDshotTelemetry && rpmFilterConfig()->gyro_rpm_notch_harmonics); } float rpmMinMotorFrequency() diff --git a/src/main/flight/rpm_filter.h b/src/main/flight/rpm_filter.h index 89895d213c..74737fda75 100644 --- a/src/main/flight/rpm_filter.h +++ b/src/main/flight/rpm_filter.h @@ -29,10 +29,6 @@ typedef struct rpmFilterConfig_s uint8_t gyro_rpm_notch_min; // minimum frequency of the notches uint16_t gyro_rpm_notch_q; // q of the notches - uint8_t dterm_rpm_notch_harmonics; // how many harmonics should be covered with notches? 0 means filter off - uint8_t dterm_rpm_notch_min; // minimum frequency of the notches - uint16_t dterm_rpm_notch_q; // q of the notches - uint16_t rpm_lpf; // the cutoff of the lpf on reported motor rpm } rpmFilterConfig_t;