Dimmable RPM harmonics (#12838)

src/main/blackbox/blackbox.c

@@ -1518,6 +1518,9 @@ static bool blackboxWriteSysinfo(void)
 #endif
 #ifdef USE_RPM_FILTER
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_HARMONICS, "%d",     rpmFilterConfig()->rpm_filter_harmonics);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_WEIGHTS, "%d,%d,%d", rpmFilterConfig()->rpm_filter_weights[0],
+                                                                              rpmFilterConfig()->rpm_filter_weights[1],
+                                                                              rpmFilterConfig()->rpm_filter_weights[2]);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_Q, "%d",             rpmFilterConfig()->rpm_filter_q);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_MIN_HZ, "%d",        rpmFilterConfig()->rpm_filter_min_hz);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, "%d", rpmFilterConfig()->rpm_filter_fade_range_hz);

src/main/cli/settings.c

@@ -1721,6 +1721,7 @@ const clivalue_t valueTable[] = {
 
 #ifdef USE_RPM_FILTER
     { PARAM_NAME_RPM_FILTER_HARMONICS,     VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_harmonics) },
+    { PARAM_NAME_RPM_FILTER_WEIGHTS,       VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_HARMONICS_MAX, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_weights) },
     { PARAM_NAME_RPM_FILTER_Q,             VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_q) },
     { PARAM_NAME_RPM_FILTER_MIN_HZ,        VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_min_hz) },
     { PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, rpm_filter_fade_range_hz) },

src/main/fc/parameter_names.h

@@ -116,6 +116,7 @@
 #define PARAM_NAME_SIMPLIFIED_GYRO_FILTER_MULTIPLIER "simplified_gyro_filter_multiplier"
 #define PARAM_NAME_DEBUG_MODE "debug_mode"
 #define PARAM_NAME_RPM_FILTER_HARMONICS "rpm_filter_harmonics"
+#define PARAM_NAME_RPM_FILTER_WEIGHTS "rpm_filter_weights"
 #define PARAM_NAME_RPM_FILTER_Q "rpm_filter_q"
 #define PARAM_NAME_RPM_FILTER_MIN_HZ "rpm_filter_min_hz"
 #define PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ "rpm_filter_fade_range_hz"

src/main/flight/rpm_filter.c

@@ -44,7 +44,6 @@
 
 #include "rpm_filter.h"
 
-#define RPM_FILTER_HARMONICS_MAX 3
 #define RPM_FILTER_DURATION_S    0.001f  // Maximum duration allowed to update all RPM notches once
 #define SECONDS_PER_MINUTE       60.0f
 #define ERPM_PER_LSB             100.0f
@@ -53,6 +52,7 @@
 typedef struct rpmFilter_s {
 
     int numHarmonics;
+    float weights[RPM_FILTER_HARMONICS_MAX];
     float minHz;
     float maxHz;
     float fadeRangeHz;
@@ -109,6 +109,10 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
     rpmFilter.q = config->rpm_filter_q / 100.0f;
     rpmFilter.looptimeUs = looptimeUs;
 
+    for (int n = 0; n < RPM_FILTER_HARMONICS_MAX; n++) {
+        rpmFilter.weights[n] = constrainf(config->rpm_filter_weights[n] / 100.0f, 0.0f, 1.0f);
+    }
+
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         for (int motor = 0; motor < getMotorCount(); motor++) {
             for (int i = 0; i < rpmFilter.numHarmonics; i++) {
@@ -145,18 +149,24 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void)
     // update RPM notches
     for (int i = 0; i < notchUpdatesPerIteration; i++) {
 
+        // Only bother updating notches which can have an effect on filtered output
+        if (rpmFilter.weights[harmonicIndex] > 0.0f) {
+
             // select current notch on ROLL
             biquadFilter_t *template = &rpmFilter.notch[0][motorIndex][harmonicIndex];
 
             const float frequencyHz = constrainf((harmonicIndex + 1) * motorFrequencyHz[motorIndex], rpmFilter.minHz, rpmFilter.maxHz);
             const float marginHz = frequencyHz - rpmFilter.minHz;
+            float weight = 1.0f;
 
             // fade out notch when approaching minHz (turn it off)
-        float weight = 1.0f;
             if (marginHz < rpmFilter.fadeRangeHz) {
-            weight = marginHz / rpmFilter.fadeRangeHz;
+                weight *= marginHz / rpmFilter.fadeRangeHz;
             }
 
+            // attenuate notches per harmonics group
+            weight *= rpmFilter.weights[harmonicIndex];
+
             // update notch
             biquadFilterUpdate(template, frequencyHz, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, weight);
 
@@ -170,6 +180,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void)
                 dest->a2 = template->a2;
                 dest->weight = template->weight;
             }
+        }
 
         // cycle through all notches on ROLL (takes RPM_FILTER_DURATION_S at max.)
         harmonicIndex = (harmonicIndex + 1) % rpmFilter.numHarmonics;
@@ -184,6 +195,11 @@ FAST_CODE float rpmFilterApply(const int axis, float value)
     // Iterate over all notches on axis and apply each one to value.
     // Order of application doesn't matter because biquads are linear time-invariant filters.
     for (int i = 0; i < rpmFilter.numHarmonics; i++) {
+
+        if (rpmFilter.weights[i] <= 0.0f) {
+            continue;  // skip harmonics which have no effect on filtered output
+        }
+
         for (int motor = 0; motor < getMotorCount(); motor++) {
             value = biquadFilterApplyDF1Weighted(&rpmFilter.notch[axis][motor][i], value);
         }

src/main/pg/rpm_filter.c

@@ -34,7 +34,8 @@ PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig,
     .rpm_filter_min_hz = 100,
     .rpm_filter_fade_range_hz = 50,
     .rpm_filter_q = 500,
-    .rpm_filter_lpf_hz = 150
+    .rpm_filter_lpf_hz = 150,
+    .rpm_filter_weights = { 100, 100, 100 },
 );
 
 #endif // USE_RPM_FILTER

src/main/pg/rpm_filter.h

@@ -24,9 +24,12 @@
 
 #include "pg/pg.h"
 
+#define RPM_FILTER_HARMONICS_MAX 3
+
 typedef struct rpmFilterConfig_s
 {
     uint8_t  rpm_filter_harmonics;     // how many harmonics should be covered with notches? 0 means filter off
+    uint8_t  rpm_filter_weights[RPM_FILTER_HARMONICS_MAX];  // effect or "weight" (0% - 100%) of each RPM filter harmonic
     uint8_t  rpm_filter_min_hz;        // minimum frequency of the notches
     uint16_t rpm_filter_fade_range_hz; // range in which to gradually turn off notches down to minHz
     uint16_t rpm_filter_q;             // q of the notches