Refactor RPM Filter (#11765)

* Refactor RPM Filter

* Refactor RPM Filter parameter group

src/main/fc/core.c

@@ -361,7 +361,7 @@ void updateArmingStatus(void)
 
 #ifdef USE_RPM_FILTER
         // USE_RPM_FILTER will only be defined if USE_DSHOT and USE_DSHOT_TELEMETRY are defined
-        // If the RPM filter is anabled and any motor isn't providing telemetry, then disable arming
+        // If the RPM filter is enabled and any motor isn't providing telemetry, then disable arming
         if (isRpmFilterEnabled() && !isDshotTelemetryActive()) {
             setArmingDisabled(ARMING_DISABLED_RPMFILTER);
         } else {

src/main/flight/mixer.c

@@ -222,8 +222,8 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
 #ifdef USE_DYN_IDLE
         if (mixerRuntime.dynIdleMinRps > 0.0f) {
             const float maxIncrease = isAirmodeActivated() ? mixerRuntime.dynIdleMaxIncrease : 0.05f;
-            float minRps = rpmMinMotorFrequency();
+            float minRps = getMinMotorFrequency();
-            DEBUG_SET(DEBUG_DYN_IDLE, 3, lrintf(minRps * 10));
+            DEBUG_SET(DEBUG_DYN_IDLE, 3, lrintf(minRps * 10.0f));
             float rpsError = mixerRuntime.dynIdleMinRps - minRps;
             // PT1 type lowpass delay and smoothing for D
             minRps = mixerRuntime.prevMinRps + mixerRuntime.minRpsDelayK * (minRps - mixerRuntime.prevMinRps);

src/main/flight/pid_init.c

@@ -251,7 +251,7 @@ void pidInit(const pidProfile_t *pidProfile)
     pidInitFilters(pidProfile);
     pidInitConfig(pidProfile);
 #ifdef USE_RPM_FILTER
-    rpmFilterInit(rpmFilterConfig());
+    rpmFilterInit(rpmFilterConfig(), gyro.targetLooptime);
 #endif
 }
 

src/main/flight/rpm_filter.c

@@ -19,18 +19,18 @@
  */
 
 
+#include <float.h>
 #include <math.h>
-#include <stdint.h>
+#include <string.h>
 
 #include "platform.h"
 
-#if defined(USE_RPM_FILTER)
+#ifdef USE_RPM_FILTER
 
 #include "build/debug.h"
 
 #include "common/filter.h"
 #include "common/maths.h"
-#include "common/time.h"
 
 #include "drivers/dshot.h"
 
@@ -45,200 +45,155 @@
 
 #include "rpm_filter.h"
 
-#define RPM_FILTER_MAXHARMONICS 3
+#define RPM_FILTER_HARMONICS_MAX 3
-#define SECONDS_PER_MINUTE      60.0f
+#define RPM_FILTER_DURATION_S    0.001f  // Maximum duration allowed to update all RPM notches once
-#define ERPM_PER_LSB            100.0f
+#define SECONDS_PER_MINUTE       60.0f
-#define MIN_UPDATE_T            0.001f
+#define ERPM_PER_LSB             100.0f
 
 
-static pt1Filter_t rpmFilters[MAX_SUPPORTED_MOTORS];
+typedef struct rpmFilter_s {
 
-typedef struct rpmNotchFilter_s {
+    int numHarmonics;
+    float minHz;
+    float maxHz;
+    float fadeRangeHz;
+    float q;
 
-    uint8_t  harmonics;
-    float    minHz;
-    float    maxHz;
-    float    fadeRangeHz;
-    float    q;
     timeUs_t looptimeUs;
+    biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_HARMONICS_MAX];
 
-    biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS];
+} rpmFilter_t;
 
-} rpmNotchFilter_t;
+// Singleton
+FAST_DATA_ZERO_INIT static rpmFilter_t rpmFilter;
 
-FAST_DATA_ZERO_INIT static float   erpmToHz;
+FAST_DATA_ZERO_INIT static pt1Filter_t motorFreqLpf[MAX_SUPPORTED_MOTORS];
-FAST_DATA_ZERO_INIT static float   filteredMotorErpm[MAX_SUPPORTED_MOTORS];
+FAST_DATA_ZERO_INIT static float motorFrequencyHz[MAX_SUPPORTED_MOTORS];
-FAST_DATA_ZERO_INIT static float   motorFrequency[MAX_SUPPORTED_MOTORS];
+FAST_DATA_ZERO_INIT static float minMotorFrequencyHz;
-FAST_DATA_ZERO_INIT static float   minMotorFrequency;
+FAST_DATA_ZERO_INIT static float erpmToHz;
-FAST_DATA_ZERO_INIT static uint8_t numberFilters;
-FAST_DATA_ZERO_INIT static uint8_t numberRpmNotchFilters;
-FAST_DATA_ZERO_INIT static uint8_t filterUpdatesPerIteration;
-FAST_DATA_ZERO_INIT static float   pidLooptime;
-FAST_DATA_ZERO_INIT static rpmNotchFilter_t filters[2];
-FAST_DATA_ZERO_INIT static rpmNotchFilter_t *gyroFilter;
 
-FAST_DATA_ZERO_INIT static uint8_t currentMotor;
+// batch processing of RPM notches
-FAST_DATA_ZERO_INIT static uint8_t currentHarmonic;
+FAST_DATA_ZERO_INIT static int notchUpdatesPerIteration;
-FAST_DATA_ZERO_INIT static uint8_t currentFilterNumber;
+FAST_DATA_ZERO_INIT static int motorIndex;
-FAST_DATA static rpmNotchFilter_t *currentFilter = &filters[0];
+FAST_DATA_ZERO_INIT static int harmonicIndex;
 
 
-
+void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
-PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 5);
-
-void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
 {
-    config->rpm_filter_harmonics = 3;
+    motorIndex = 0;
-    config->rpm_filter_min_hz = 100;
+    harmonicIndex = 0;
-    config->rpm_filter_fade_range_hz = 50;
+    minMotorFrequencyHz = 0;
-    config->rpm_filter_q = 500;
+    rpmFilter.numHarmonics = 0; // disable RPM Filtering
 
-    config->rpm_filter_lpf_hz = 150;
+    // if bidirectional DShot is not available
-}
-
-static void rpmNotchFilterInit(rpmNotchFilter_t *filter, const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
-{
-    filter->harmonics = config->rpm_filter_harmonics;
-    filter->minHz = config->rpm_filter_min_hz;
-    filter->maxHz = 0.48f * 1e6f / looptimeUs; // don't go quite to nyquist to avoid oscillations
-    filter->fadeRangeHz = config->rpm_filter_fade_range_hz;
-    filter->q = config->rpm_filter_q / 100.0f;
-    filter->looptimeUs = looptimeUs;
-
-    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        for (int motor = 0; motor < getMotorCount(); motor++) {
-            for (int i = 0; i < filter->harmonics; i++) {
-                biquadFilterInit(
-                    &filter->notch[axis][motor][i], filter->minHz * i, filter->looptimeUs, filter->q, FILTER_NOTCH, 0.0f);
-            }
-        }
-    }
-}
-
-void rpmFilterInit(const rpmFilterConfig_t *config)
-{
-    currentFilter = &filters[0];
-    currentMotor = currentHarmonic = currentFilterNumber = 0;
-
-    numberRpmNotchFilters = 0;
     if (!motorConfig()->dev.useDshotTelemetry) {
-        gyroFilter = NULL;
         return;
     }
 
-    pidLooptime = gyro.targetLooptime;
+    // init LPFs for RPM data
-    if (config->rpm_filter_harmonics) {
+    for (int i = 0; i < getMotorCount(); i++) {
-        gyroFilter = &filters[numberRpmNotchFilters++];
+        pt1FilterInit(&motorFreqLpf[i], pt1FilterGain(config->rpm_filter_lpf_hz, looptimeUs * 1e-6f));
-        rpmNotchFilterInit(gyroFilter, config, pidLooptime);
-    } else {
-        gyroFilter = NULL;
     }
 
-    for (int i = 0; i < getMotorCount(); i++) {
+    // if RPM Filtering is configured to be off
-        pt1FilterInit(&rpmFilters[i], pt1FilterGain(config->rpm_filter_lpf_hz, pidLooptime * 1e-6f));
+    if (!config->rpm_filter_harmonics) {
+        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 axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        for (int motor = 0; motor < getMotorCount(); motor++) {
+            for (int i = 0; i < rpmFilter.numHarmonics; i++) {
+                biquadFilterInit(&rpmFilter.notch[axis][motor][i], rpmFilter.minHz * i, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, 0.0f);
+            }
+        }
     }
 
     erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE  / (motorConfig()->motorPoleCount / 2.0f);
 
-    const float loopIterationsPerUpdate = MIN_UPDATE_T / (pidLooptime * 1e-6f);
+    const float loopIterationsPerUpdate = RPM_FILTER_DURATION_S / (looptimeUs * 1e-6f);
-    numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics);
+    const float numNotchesPerAxis = getMotorCount() * rpmFilter.numHarmonics;
-    const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate;
+    notchUpdatesPerIteration = ceilf(numNotchesPerAxis / loopIterationsPerUpdate); // round to ceiling
-    filterUpdatesPerIteration = lrintf(filtersPerLoopIteration + 0.49f);
-}
-
-static float applyFilter(rpmNotchFilter_t *filter, const int axis, float value)
-{
-    if (filter == NULL) {
-        return value;
-    }
-    for (int motor = 0; motor < getMotorCount(); motor++) {
-        for (int i = 0; i < filter->harmonics; i++) {
-            value = biquadFilterApplyDF1Weighted(&filter->notch[axis][motor][i], value);
-        }
-    }
-    return value;
-}
-
-float rpmFilterGyro(const int axis, float value)
-{
-    return applyFilter(gyroFilter, axis, value);
 }
 
 FAST_CODE_NOINLINE void rpmFilterUpdate(void)
 {
-    for (int motor = 0; motor < getMotorCount(); motor++) {
+    if (!motorConfig()->dev.useDshotTelemetry) {
-        filteredMotorErpm[motor] = pt1FilterApply(&rpmFilters[motor], getDshotTelemetry(motor));
-        if (motor < 4) {
-            DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequency[motor]);
-        }
-        motorFrequency[motor] = erpmToHz * filteredMotorErpm[motor];
-    }
-
-    if (gyroFilter == NULL) {
-        minMotorFrequency = 0.0f;
         return;
     }
 
-    for (int i = 0; i < filterUpdatesPerIteration; i++) {
+    // update motor RPM data
+    minMotorFrequencyHz = FLT_MAX;
+    for (int motor = 0; motor < getMotorCount(); motor++) {
+        motorFrequencyHz[motor] = pt1FilterApply(&motorFreqLpf[motor], erpmToHz * getDshotTelemetry(motor));
+        minMotorFrequencyHz = MIN(minMotorFrequencyHz, motorFrequencyHz[motor]);
+        if (motor < 4) {
+            DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequencyHz[motor]);
+        }
+    }
 
-        float frequency = constrainf(
+    if (!isRpmFilterEnabled()) {
-            (currentHarmonic + 1) * motorFrequency[currentMotor], currentFilter->minHz, currentFilter->maxHz);
+        return;
-        biquadFilter_t *template = &currentFilter->notch[0][currentMotor][currentHarmonic];
+    }
-        // uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above
+
-        /* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */
+    // update RPM notches
-        /* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */
+    for (int i = 0; i < notchUpdatesPerIteration; i++) {
-        /* DEBUG_SET(DEBUG_RPM_FILTER, 2, currentFilter == &gyroFilter); */
+
-        /* DEBUG_SET(DEBUG_RPM_FILTER, 3, frequency) */
+        // 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;
         
         // fade out notch when approaching minHz (turn it off)
         float weight = 1.0f;
-        if (frequency < currentFilter->minHz + currentFilter->fadeRangeHz) {
+        if (marginHz < rpmFilter.fadeRangeHz) {
-            weight = (frequency - currentFilter->minHz) / currentFilter->fadeRangeHz;
+            weight = marginHz / rpmFilter.fadeRangeHz;
         }
 
-        biquadFilterUpdate(
+        // update notch
-            template, frequency, currentFilter->looptimeUs, currentFilter->q, FILTER_NOTCH, weight);
+        biquadFilterUpdate(template, frequencyHz, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, weight);
 
+        // copy notch properties to corresponding notches (clones) on PITCH and YAW
         for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilter_t *clone = &currentFilter->notch[axis][currentMotor][currentHarmonic];
+            biquadFilter_t *clone = &rpmFilter.notch[axis][motorIndex][harmonicIndex];
-            clone->b0 = template->b0;
+            memcpy(clone, template, sizeof(biquadFilter_t));
-            clone->b1 = template->b1;
-            clone->b2 = template->b2;
-            clone->a1 = template->a1;
-            clone->a2 = template->a2;
-            clone->weight = template->weight;
         }
 
-        if (++currentHarmonic == currentFilter->harmonics) {
+        // cycle through all notches on ROLL (takes RPM_FILTER_DURATION_S at max.)
-            currentHarmonic = 0;
+        harmonicIndex = (harmonicIndex + 1) % rpmFilter.numHarmonics;
-            if (++currentFilterNumber == numberRpmNotchFilters) {
+        if (harmonicIndex == 0) {
-                currentFilterNumber = 0;
+            motorIndex = (motorIndex + 1) % getMotorCount();
-                if (++currentMotor == getMotorCount()) {
-                    currentMotor = 0;
-                }
-                minMotorFrequency = 0.0f;
-            }
-            currentFilter = &filters[currentFilterNumber];
         }
     }
 }
 
+FAST_CODE float rpmFilterApply(const int axis, float value)
+{
+    for (int i = 0; i < rpmFilter.numHarmonics; i++) {
+        for (int motor = 0; motor < getMotorCount(); motor++) {
+            value = biquadFilterApplyDF1Weighted(&rpmFilter.notch[axis][motor][i], value);
+        }
+    }
+
+    return value;
+}
+
 bool isRpmFilterEnabled(void)
 {
-    return (motorConfig()->dev.useDshotTelemetry && rpmFilterConfig()->rpm_filter_harmonics);
+    return rpmFilter.numHarmonics > 0;
 }
 
-float rpmMinMotorFrequency(void)
+float getMinMotorFrequency(void)
 {
-    if (minMotorFrequency == 0.0f) {
+    return minMotorFrequencyHz;
-        minMotorFrequency = 10000.0f; // max RPM reported in Hz = 600,000RPM
-        for (int i = getMotorCount(); i--;) {
-            if (motorFrequency[i] < minMotorFrequency) {
-                minMotorFrequency = motorFrequency[i];
-            }
-        }
-    }
-    return minMotorFrequency;
 }
 
-#endif
+#endif // USE_RPM_FILTER

src/main/flight/rpm_filter.h

@@ -20,24 +20,14 @@
 
 #pragma once
 
-#include "common/axis.h"
+#include <stdbool.h>
-#include "pg/pg.h"
 
-typedef struct rpmFilterConfig_s
+#include "common/time.h"
-{
-    uint8_t  rpm_filter_harmonics;     // how many harmonics should be covered with notches? 0 means filter off
-    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
 
-    uint16_t rpm_filter_lpf_hz;        // the cutoff of the lpf on reported motor rpm
+#include "pg/rpm_filter.h"
 
-} rpmFilterConfig_t;
+void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs);
-
+void rpmFilterUpdate(void);
-PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);
+float rpmFilterApply(const int axis, float value);
-
-void  rpmFilterInit(const rpmFilterConfig_t *config);
-float rpmFilterGyro(const int axis, float value);
-void  rpmFilterUpdate(void);
 bool isRpmFilterEnabled(void);
-float rpmMinMotorFrequency(void);
+float getMinMotorFrequency(void);

src/main/pg/rpm_filter.c

@@ -0,0 +1,40 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#ifdef USE_RPM_FILTER
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "rpm_filter.h"
+
+PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 5);
+
+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,
+    .rpm_filter_lpf_hz = 150
+);
+
+#endif // USE_RPM_FILTER

src/main/pg/rpm_filter.h

@@ -0,0 +1,38 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include "pg/pg.h"
+
+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_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
+
+    uint16_t rpm_filter_lpf_hz;        // the cutoff of the lpf on reported motor rpm
+
+} rpmFilterConfig_t;
+
+PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);

src/main/sensors/gyro_filter_impl.c

@@ -50,7 +50,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
         GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 1, lrintf(gyroADCf));
 
 #ifdef USE_RPM_FILTER
-        gyroADCf = rpmFilterGyro(axis, gyroADCf);
+        gyroADCf = rpmFilterApply(axis, gyroADCf);
 #endif
 
         // DEBUG_GYRO_SAMPLE(2) Record the post-RPM Filter value for the selected debug axis