Removed DTerm RPM filter.

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)

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
 

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"

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;

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()

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;