increase possible Q to 30, default Q to 10 and set scheduler optimize rate policy by default

src/main/cli/settings.c

@@ -1411,11 +1411,12 @@ const clivalue_t valueTable[] = {
 
 #ifdef USE_RPM_FILTER
     { "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 = { 1, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) },
+    { "gyro_rpm_notch_q",  VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 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 = { 1, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_q) },
+    { "dterm_rpm_notch_q",  VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 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_lpf",  VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 500 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_lpf) },
 #endif
 
 #ifdef USE_RX_FLYSKY

src/main/fc/config.c

@@ -63,6 +63,7 @@
 #include "sensors/acceleration.h"
 #include "sensors/battery.h"
 #include "sensors/gyro.h"
+#include "sensors/rpm_filter.h"
 
 #include "scheduler/scheduler.h"
 
@@ -337,6 +338,11 @@ static void validateAndFixConfig(void)
     }
 #endif
 
+#if defined(USE_DSHOT_TELEMETRY)
+    systemConfigMutable()->schedulerOptimizeRate = systemConfigMutable()->schedulerOptimizeRate ||
+        (rpmFilterConfig()->gyro_rpm_notch_harmonics + rpmFilterConfig()->dterm_rpm_notch_harmonics);
+#endif
+
 // clear features that are not supported.
 // I have kept them all here in one place, some could be moved to sections of code above.
 

src/main/sensors/rpm_filter.c

@@ -24,15 +24,16 @@
 #include "platform.h"
 #include "build/debug.h"
 #include "common/filter.h"
+#include "common/maths.h"
 #include "drivers/pwm_output_counts.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "pg/pg_ids.h"
+#include "scheduler/scheduler.h"
 #include "sensors/rpm_filter.h"
 #include "sensors/gyro.h"
 
 #define RPM_FILTER_MAXHARMONICS 3
-#define RPM_MOTOR_FILTER_CUTOFF 150
 #define SECONDS_PER_MINUTE      60.0f
 #define ERPM_PER_LSB            100.0f
 #define MIN_UPDATE_T            0.001f
@@ -61,7 +62,7 @@ static rpmNotchFilter_t filters[2];
 static rpmNotchFilter_t* gyroFilter;
 static rpmNotchFilter_t* dtermFilter;
 
-PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 2);
+PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 3);
 
 void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
 {
@@ -69,9 +70,11 @@ void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
     config->gyro_rpm_notch_min = 100;
     config->gyro_rpm_notch_q = 500;
 
-    config->dterm_rpm_notch_harmonics = 1;
+    config->dterm_rpm_notch_harmonics = 0;
     config->dterm_rpm_notch_min = 100;
     config->dterm_rpm_notch_q = 500;
+
+    config->rpm_lpf = 150;
 }
 
 static void rpmNotchFilterInit(rpmNotchFilter_t* filter, int harmonics, int minHz, int q, float looptime)
@@ -112,7 +115,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
     }
 
     for (int i = 0; i < getMotorCount(); i++) {
-        pt1FilterInit(&rpmFilters[i], pt1FilterGain(RPM_MOTOR_FILTER_CUTOFF, pidLooptime));
+        pt1FilterInit(&rpmFilters[i], pt1FilterGain(config->rpm_lpf, pidLooptime));
     }
 
     erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE  / (motorConfig()->motorPoleCount / 2.0f);
@@ -121,6 +124,9 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
     numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics);
     const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate;
     filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f);
+    if (gyroFilter || dtermFilter) {
+        schedulerOptimizeRate(true);
+    }
 }
 
 static float applyFilter(rpmNotchFilter_t* filter, int axis, float value)
@@ -168,19 +174,7 @@ void rpmFilterUpdate()
 
     for (int i = 0; i < filterUpdatesPerIteration; i++) {
 
-        float frequency = (harmonic + 1) * motorFrequency[motor];
+        float frequency = constrainf((harmonic + 1) * motorFrequency[motor], 0.0f, currentFilter->minHz);
-        const float deactivateFreq = 1000.0f;
-        if (frequency < currentFilter->minHz) {
-            if (frequency < 0.5f * currentFilter->minHz) {
-                frequency = deactivateFreq;
-            } else {
-                frequency = currentFilter->minHz;
-            }
-        }
-        if (frequency > deactivateFreq) {
-            frequency = deactivateFreq;
-        }
-
         biquadFilter_t* template = &currentFilter->notch[0][motor][harmonic];
         // uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above
         /* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */

src/main/sensors/rpm_filter.h

@@ -33,6 +33,7 @@ typedef struct rpmFilterConfig_s
     uint8_t  dterm_rpm_notch_min;        // minimum frequency of the notches
     uint16_t dterm_rpm_notch_q;          // q of the notches
 
+    uint8_t  rpm_lpf;                    // the cutoff of the lpf on reported motor rpm
 } rpmFilterConfig_t;
 
 PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);