Merge bfff81889c into 9458ac043a

src/main/blackbox/blackbox.c

@@ -1700,7 +1700,6 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_POLES, "%d",            motorConfig()->motorPoleCount);
 #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]);

src/main/cli/settings.c

@@ -1936,7 +1936,6 @@ const clivalue_t valueTable[] = {
 #endif
 
 #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) },

src/main/fc/parameter_names.h

@@ -150,7 +150,6 @@
 #define PARAM_NAME_SIMPLIFIED_GYRO_FILTER "simplified_gyro_filter"
 #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"

src/main/flight/rpm_filter.c

@@ -46,7 +46,7 @@
 
 typedef struct rpmFilter_s {
 
-    int numHarmonics;
+    int activeHarmonicsCount;
     float weights[RPM_FILTER_HARMONICS_MAX];
     float minHz;
     float maxHz;
@@ -70,46 +70,48 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
 {
     motorIndex = 0;
     harmonicIndex = 0;
-    rpmFilter.numHarmonics = 0; // disable RPM Filtering
+    rpmFilter.activeHarmonicsCount = 0; // disable RPM Filtering
 
     // if bidirectional DShot is not available
     if (!useDshotTelemetry) {
         return;
     }
     
+    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);
+        if (rpmFilter.weights[n] > 0.0f) {
+            rpmFilter.activeHarmonicsCount++;
+        }
+    }
+
     // if RPM Filtering is configured to be off
-    if (!config->rpm_filter_harmonics) {
+    if (!isRpmFilterEnabled()) {
         return;
     }
 
     // if we get to this point, enable and init RPM filtering
-    rpmFilter.numHarmonics = config->rpm_filter_harmonics;
     rpmFilter.minHz = config->rpm_filter_min_hz;
     rpmFilter.maxHz = 0.48f * 1e6f / looptimeUs; // don't go quite to nyquist to avoid oscillations
     rpmFilter.fadeRangeHz = config->rpm_filter_fade_range_hz;
     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++) {
+            for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
-                biquadFilterInit(&rpmFilter.notch[axis][motor][i], rpmFilter.minHz * i, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, 0.0f);
+                biquadFilterInit(&rpmFilter.notch[axis][motor][i], rpmFilter.minHz * (i + 1), rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, rpmFilter.weights[i]);
             }
         }
     }
 
     const float loopIterationsPerUpdate = RPM_FILTER_DURATION_S / (looptimeUs * 1e-6f);
-    const float numNotchesPerAxis = getMotorCount() * rpmFilter.numHarmonics;
+    const float numNotchesPerAxis = getMotorCount() * rpmFilter.activeHarmonicsCount;
     notchUpdatesPerIteration = ceilf(numNotchesPerAxis / loopIterationsPerUpdate); // round to ceiling
 }
 
 FAST_CODE_NOINLINE void rpmFilterUpdate(void)
 {
-    if (!useDshotTelemetry) {
+    if (!isRpmFilterEnabled()) {
         return;
     }
 
@@ -161,34 +163,31 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void)
         }
 
         // cycle through all notches on ROLL (takes RPM_FILTER_DURATION_S at max.)
-        harmonicIndex = (harmonicIndex + 1) % rpmFilter.numHarmonics;
+        //not sure if it correct
-        if (harmonicIndex == 0) {
+        motorIndex++;
-            motorIndex = (motorIndex + 1) % getMotorCount();
+        if (motorIndex >= getMotorCount()) {
+            // All motors processed for this harmonic, move to next harmonic
+            motorIndex = 0;
+            harmonicIndex = (harmonicIndex + 1) % RPM_FILTER_HARMONICS_MAX;
         }
     }
 }
 
 FAST_CODE float rpmFilterApply(const int axis, float value)
 {
-    // Iterate over all notches on axis and apply each one to value.
+    for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
-    // Order of application doesn't matter because biquads are linear time-invariant filters.
+        if (rpmFilter.weights[i] > 0.0f) {
-    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);
             }
         }
-
+    }
     return value;
 }
 
 bool isRpmFilterEnabled(void)
 {
-    return rpmFilter.numHarmonics > 0;
+    return rpmFilter.activeHarmonicsCount > 0;
 }
 
 #endif // USE_RPM_FILTER

src/main/msp/msp.c

@@ -1911,10 +1911,15 @@ case MSP_NAME:
         sbufWriteU16(dst, 0);
 #endif
 #if defined(USE_RPM_FILTER)
-        sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_harmonics);
+        for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
+            // Added in MSP API 1.46
+            sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_weights[i]);
+        }
         sbufWriteU8(dst, rpmFilterConfig()->rpm_filter_min_hz);
 #else
+        for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
                 sbufWriteU8(dst, 0);
+        }
         sbufWriteU8(dst, 0);
 #endif
 #if defined(USE_DYN_NOTCH_FILTER)
@@ -3107,7 +3112,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
             sbufReadU16(src);
 #endif
         }
-        if (sbufBytesRemaining(src) >= 8) {
+        if (sbufBytesRemaining(src) >= 10) {
             // Added in MSP API 1.42
 #if defined(USE_DYN_NOTCH_FILTER)
             sbufReadU8(src); // DEPRECATED 1.43: dyn_notch_range
@@ -3121,10 +3126,15 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
             sbufReadU16(src);
 #endif
 #if defined(USE_RPM_FILTER)
-            rpmFilterConfigMutable()->rpm_filter_harmonics = sbufReadU8(src);
+            for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
+                // Added in MSP API 1.46
+                rpmFilterConfigMutable()->rpm_filter_weights[i] = sbufReadU8(src);
+            }
             rpmFilterConfigMutable()->rpm_filter_min_hz = sbufReadU8(src);
 #else
+            for (int i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) {
                 sbufReadU8(src);
+            }
             sbufReadU8(src);
 #endif
         }

src/main/pg/rpm_filter.c

@@ -30,7 +30,6 @@
 PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 6);
 
 PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig,
-    .rpm_filter_harmonics = 3,
     .rpm_filter_min_hz = 100,
     .rpm_filter_fade_range_hz = 50,
     .rpm_filter_q = 500,

src/main/pg/rpm_filter.h

@@ -28,7 +28,6 @@
 
 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