RPM filters (#5188)

* Empty framework to apply RPM filter

* Init gyro RPM filter

* Entry point for Dterm filter

* RPM filter implementation

* Bugfixes

* Fix Dterm fileter bank

* Save RPM filter configuration in blackbox header

* Debug RPM frequency

* Disable PWM servo driver on all F3 boards

* Move RPM filter to ITCM_RAM

* Disable target COLIBRI_RACE as it's out of RAM

* Drop FEATURE in favor of just settings

Makefile

@@ -126,7 +126,7 @@ else
 $(error Unknown target MCU specified.)
 endif
 
-GROUP_1_TARGETS := AIRHEROF3 AIRHEROF3_QUAD COLIBRI_RACE LUX_RACE SPARKY REVO SPARKY2 COLIBRI FALCORE FF_F35_LIGHTNING FF_FORTINIF4 FF_PIKOF4 FF_PIKOF4OSD
+GROUP_1_TARGETS := AIRHEROF3 AIRHEROF3_QUAD LUX_RACE SPARKY REVO SPARKY2 COLIBRI FALCORE FF_F35_LIGHTNING FF_FORTINIF4 FF_PIKOF4 FF_PIKOF4OSD
 GROUP_2_TARGETS := SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO CLRACINGF4AIR CLRACINGF4AIRV2 BEEROTORF4 BETAFLIGHTF3 BETAFLIGHTF4 PIKOBLX
 GROUP_3_TARGETS := OMNIBUS AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO FIREWORKSV2 SPARKY2 MATEKF405 OMNIBUSF7 DYSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 OMNIBUSF7NXT OMNIBUSF7V2 ASGARD32F4
 GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ALIENFLIGHTNGF7 PIXRACER YUPIF7 MATEKF405SE MATEKF411 MATEKF722 MATEKF405OSD MATEKF405_SERVOS6 NOX

fake_travis_build.sh

@@ -2,7 +2,6 @@
 
 targets=("PUBLISHMETA=True" \
     "RUNTESTS=True" \
-    "TARGET=COLIBRI_RACE" \
     "TARGET=SPRACINGF3" \
     "TARGET=SPRACINGF3EVO" \
     "TARGET=LUX_RACE" \

make/release.mk

@@ -1,6 +1,6 @@
 RELEASE_TARGETS = AIRHEROF3 AIRHEROF3_QUAD
 
-RELEASE_TARGETS += COLIBRI_RACE LUX_RACE FURYF3 FURYF3_SPIFLASH RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405
+RELEASE_TARGETS += LUX_RACE FURYF3 FURYF3_SPIFLASH RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405
 RELEASE_TARGETS += QUANTON REVO SPARKY2 PIKOBLX CLRACINGF4AIR CLRACINGF4AIRV2 PIXRACER BEEROTORF4 ANYFCF7 ANYFCF7_EXTERNAL_BARO
 RELEASE_TARGETS += ALIENFLIGHTNGF7
 

make/source.mk

@@ -97,6 +97,7 @@ COMMON_SRC = \
             flight/servos.c \
             flight/wind_estimator.c \
             flight/gyroanalyse.c \
+            flight/rpm_filter.c \
             io/beeper.c \
             io/esc_serialshot.c \
             io/frsky_osd.c \

src/main/blackbox/blackbox.c

@@ -58,6 +58,7 @@
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/servos.h"
+#include "flight/rpm_filter.h"
 
 #include "io/beeper.h"
 #include "io/gps.h"
@@ -1711,6 +1712,16 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d",                     pidProfile()->pidSumLimit);
         BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitYaw", "%d",        pidProfile()->axisAccelerationLimitYaw);
         BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitRollPitch", "%d",  pidProfile()->axisAccelerationLimitRollPitch);
+#ifdef USE_RPM_FILTER
+        BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_filter_enabled", "%d",         rpmFilterConfig()->gyro_filter_enabled);
+        BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_harmonics", "%d",              rpmFilterConfig()->gyro_harmonics);
+        BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_min_hz", "%d",                 rpmFilterConfig()->gyro_min_hz);
+        BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_q", "%d",                      rpmFilterConfig()->gyro_q);
+        BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_filter_enabled", "%d",        rpmFilterConfig()->dterm_filter_enabled);
+        BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_harmonics", "%d",             rpmFilterConfig()->dterm_harmonics);
+        BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_min_hz", "%d",                rpmFilterConfig()->dterm_min_hz);
+        BLACKBOX_PRINT_HEADER_LINE("rpm_dterm_q", "%d",                     rpmFilterConfig()->dterm_q);
+#endif
         default:
             return true;
     }

src/main/build/debug.h

@@ -65,5 +65,7 @@ typedef enum {
     DEBUG_ACC,
     DEBUG_ITERM_RELAX,
     DEBUG_ERPM,
+    DEBUG_RPM_FILTER,
+    DEBUG_RPM_FREQ,
     DEBUG_COUNT
 } debugType_e;

src/main/config/parameter_group_ids.h

@@ -109,7 +109,8 @@
 #define PG_GENERAL_SETTINGS 1019
 #define PG_GLOBAL_FUNCTIONS 1020
 #define PG_ESC_SENSOR_CONFIG 1021
-#define PG_INAV_END 1021
+#define PG_RPM_FILTER_CONFIG 1022
+#define PG_INAV_END 1022
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/fc/cli.c

@@ -144,7 +144,7 @@ static bool commandBatchError = false;
 // sync this with features_e
 static const char * const featureNames[] = {
     "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP",
-    "DYNAMIC_FILTERS", "SOFTSERIAL", "GPS", "",
+    "DYNAMIC_FILTERS", "SOFTSERIAL", "GPS", "RPM_FILTERS",
     "", "TELEMETRY", "CURRENT_METER", "3D", "",
     "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
     "BLACKBOX", "", "TRANSPONDER", "AIRMODE",

src/main/fc/config.h

@@ -44,7 +44,7 @@ typedef enum {
     FEATURE_DYNAMIC_FILTERS = 1 << 5,   // was FEATURE_SERVO_TILT
     FEATURE_SOFTSERIAL = 1 << 6,
     FEATURE_GPS = 1 << 7,
-    FEATURE_UNUSED_3 = 1 << 8,          // was FEATURE_FAILSAFE
+    FEATURE_UNUSED_3 = 1 << 8,        // was FEATURE_FAILSAFE
     FEATURE_UNUSED_4 = 1 << 9,          // was FEATURE_SONAR
     FEATURE_TELEMETRY = 1 << 10,
     FEATURE_CURRENT_METER = 1 << 11,

src/main/fc/fc_init.c

@@ -90,6 +90,7 @@
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/servos.h"
+#include "flight/rpm_filter.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/beeper.h"
@@ -644,5 +645,13 @@ void init(void)
     }
 #endif
 
+#ifdef USE_RPM_FILTER
+    disableRpmFilters();
+    if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) {
+        rpmFiltersInit();
+        setTaskEnabled(TASK_RPM_FILTER, true);
+    }
+#endif
+
     systemState |= SYSTEM_STATE_READY;
 }

src/main/fc/fc_tasks.c

@@ -47,6 +47,7 @@
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/wind_estimator.h"
+#include "flight/rpm_filter.h"
 
 #include "navigation/navigation.h"
 
@@ -561,4 +562,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif
+#ifdef USE_RPM_FILTER
+    [TASK_RPM_FILTER] = {
+        .taskName = "RPM",
+        .taskFunc = rpmFilterUpdateTask,
+        .desiredPeriod = TASK_PERIOD_HZ(RPM_FILTER_UPDATE_RATE_HZ),          // 300Hz @3,33ms
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
 };

src/main/fc/settings.c

@@ -9,6 +9,7 @@
 #include "fc/settings.h"
 
 #include "config/general_settings.h"
+#include "flight/rpm_filter.h"
 #include "settings_generated.c"
 
 static bool settingGetWord(char *buf, int idx)

src/main/fc/settings.yaml

@@ -81,7 +81,7 @@ tables:
     values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX", 
-      "ERPM"]
+      "ERPM", "RPM_FILTER", "RPM_FREQ"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -871,6 +871,46 @@ groups:
         min: 0
         max: 3
 
+  - name: PG_RPM_FILTER_CONFIG
+    condition: USE_RPM_FILTER
+    type: rpmFilterConfig_t
+    members:
+      - name: rpm_gyro_filter_enabled
+        field: gyro_filter_enabled
+        type: bool
+      - name: rpm_dterm_filter_enabled
+        field: dterm_filter_enabled
+        type: bool
+      - name: rpm_gyro_harmonics
+        field: gyro_harmonics
+        type: uint8_t
+        min: 1
+        max: 3
+      - name: rpm_gyro_min_hz
+        field: gyro_min_hz
+        type: uint8_t
+        min: 50
+        max: 200
+      - name: rpm_gyro_q
+        field: gyro_q
+        type: uint16_t
+        min: 1
+        max: 3000
+      - name: dterm_gyro_harmonics
+        field: dterm_harmonics
+        type: uint8_t
+        min: 1
+        max: 3
+      - name: rpm_dterm_min_hz
+        field: dterm_min_hz
+        type: uint8_t
+        min: 50
+        max: 200
+      - name: rpm_dterm_q
+        field: dterm_q
+        type: uint16_t
+        min: 1
+        max: 3000
   - name: PG_GPS_CONFIG
     type: gpsConfig_t
     condition: USE_GPS

src/main/flight/pid.c

@@ -41,6 +41,7 @@
 #include "flight/pid.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
+#include "flight/rpm_filter.h"
 
 #include "io/gps.h"
 
@@ -696,6 +697,10 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
         // Apply D-term notch
         deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered);
 
+#ifdef USE_RPM_FILTER
+        deltaFiltered = rpmFilterDtermApply((uint8_t)axis, deltaFiltered);
+#endif
+
         // Apply additional lowpass
         deltaFiltered = dTermLpfFilterApplyFn(&pidState->deltaLpfState, deltaFiltered);
 

src/main/flight/rpm_filter.c

@@ -0,0 +1,232 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it 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.
+ *
+ * This file is distributed in the hope that it 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 program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "flight/rpm_filter.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/utils.h"
+#include "common/maths.h"
+#include "common/filter.h"
+#include "flight/mixer.h"
+#include "sensors/esc_sensor.h"
+#include "fc/config.h"
+
+#ifdef USE_RPM_FILTER
+
+#define RPM_TO_HZ 60.0f
+#define RPM_FILTER_RPM_LPF_HZ 150
+#define RPM_FILTER_HARMONICS 3
+
+PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 0);
+
+PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig,
+                  .gyro_filter_enabled = 0,
+                  .dterm_filter_enabled = 0,
+                  .gyro_harmonics = 1,
+                  .gyro_min_hz = 100,
+                  .gyro_q = 500,
+                  .dterm_harmonics = 1,
+                  .dterm_min_hz = 100,
+                  .dterm_q = 500, );
+
+typedef struct
+{
+    float q;
+    float minHz;
+    float maxHz;
+    uint8_t harmonics;
+    biquadFilter_t filters[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_HARMONICS];
+} rpmFilterBank_t;
+
+typedef float (*rpmFilterApplyFnPtr)(rpmFilterBank_t *filter, uint8_t axis, float input);
+typedef void (*rpmFilterUpdateFnPtr)(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency);
+
+static EXTENDED_FASTRAM pt1Filter_t motorFrequencyFilter[MAX_SUPPORTED_MOTORS];
+static EXTENDED_FASTRAM float erpmToHz;
+static EXTENDED_FASTRAM rpmFilterBank_t gyroRpmFilters;
+static EXTENDED_FASTRAM rpmFilterBank_t dtermRpmFilters;
+static EXTENDED_FASTRAM rpmFilterApplyFnPtr rpmGyroApplyFn;
+static EXTENDED_FASTRAM rpmFilterApplyFnPtr rpmDtermApplyFn;
+static EXTENDED_FASTRAM rpmFilterUpdateFnPtr rpmGyroUpdateFn;
+static EXTENDED_FASTRAM rpmFilterUpdateFnPtr rpmDtermUpdateFn;
+
+float nullRpmFilterApply(rpmFilterBank_t *filter, uint8_t axis, float input)
+{
+    UNUSED(filter);
+    UNUSED(axis);
+    return input;
+}
+
+void nullRpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency) {
+    UNUSED(filterBank);
+    UNUSED(motor);
+    UNUSED(baseFrequency);
+}
+
+float FAST_CODE rpmFilterApply(rpmFilterBank_t *filterBank, uint8_t axis, float input)
+{
+    float output = input;
+
+    for (uint8_t motor = 0; motor < getMotorCount(); motor++)
+    {
+        for (int harmonicIndex = 0; harmonicIndex < filterBank->harmonics; harmonicIndex++)
+        {
+            output = biquadFilterApplyDF1(
+                &filterBank->filters[axis][motor][harmonicIndex],
+                output
+            );
+        }
+    }
+
+    return output;
+}
+
+static void rpmFilterInit(rpmFilterBank_t *filter, uint16_t q, uint8_t minHz, uint8_t harmonics)
+{
+    filter->q = q / 100.0f;
+    filter->minHz = minHz;
+    filter->harmonics = harmonics;
+    /*
+     * Max frequency has to be lower than Nyquist frequency for looptime
+     */
+    filter->maxHz = 0.48f * 1000000.0f / getLooptime();
+
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
+    {
+        for (int motor = 0; motor < getMotorCount(); motor++)
+        {
+
+            /*
+             * Harmonics are indexed from 1 where 1 means base frequency
+             * C indexes arrays from 0, so we need to shift
+             */
+            for (int harmonicIndex = 0; harmonicIndex < harmonics; harmonicIndex++)
+            {
+                biquadFilterInit(
+                    &filter->filters[axis][motor][harmonicIndex],
+                    filter->minHz * (harmonicIndex + 1),
+                    getLooptime(),
+                    filter->q,
+                    FILTER_NOTCH);
+            }
+        }
+    }
+}
+
+void disableRpmFilters(void) {
+    rpmGyroApplyFn = (rpmFilterApplyFnPtr)nullRpmFilterApply;
+    rpmDtermApplyFn = (rpmFilterApplyFnPtr)nullRpmFilterApply;
+}
+
+void FAST_CODE NOINLINE rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency)
+{
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
+    {
+        for (int harmonicIndex = 0; harmonicIndex < filterBank->harmonics; harmonicIndex++)
+        {
+            float harmonicFrequency = baseFrequency * (harmonicIndex + 1);
+            harmonicFrequency = constrainf(harmonicFrequency, filterBank->minHz, filterBank->maxHz);
+
+            biquadFilterUpdate(
+                &filterBank->filters[axis][motor][harmonicIndex],
+                harmonicFrequency,
+                getLooptime(),
+                filterBank->q,
+                FILTER_NOTCH);
+        }
+    }
+}
+
+void rpmFiltersInit(void)
+{
+    for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++)
+    {
+        pt1FilterInit(&motorFrequencyFilter[i], RPM_FILTER_RPM_LPF_HZ, RPM_FILTER_UPDATE_RATE_US * 1e-6f);
+    }
+    erpmToHz = ERPM_PER_LSB / (motorConfig()->motorPoleCount / 2) / RPM_TO_HZ;
+
+    rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate;
+    rpmDtermUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate;
+
+    if (rpmFilterConfig()->gyro_filter_enabled)
+    {
+        rpmFilterInit(
+            &gyroRpmFilters,
+            rpmFilterConfig()->gyro_q,
+            rpmFilterConfig()->gyro_min_hz,
+            rpmFilterConfig()->gyro_harmonics);
+        rpmGyroApplyFn = (rpmFilterApplyFnPtr)rpmFilterApply;
+        rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)rpmFilterUpdate;
+    }
+
+    if (rpmFilterConfig()->dterm_filter_enabled)
+    {
+        rpmFilterInit(
+            &dtermRpmFilters,
+            rpmFilterConfig()->dterm_q,
+            rpmFilterConfig()->dterm_min_hz,
+            rpmFilterConfig()->dterm_harmonics);
+        rpmDtermApplyFn = (rpmFilterApplyFnPtr)rpmFilterApply;
+        rpmDtermUpdateFn = (rpmFilterUpdateFnPtr)rpmFilterUpdate;
+    }
+}
+
+void FAST_CODE NOINLINE rpmFilterUpdateTask(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+
+    uint8_t motorCount = getMotorCount();
+    /*
+     * For each motor, read ERPM, filter it and update motor frequency
+     */
+    for (uint8_t i = 0; i < motorCount; i++)
+    {
+        const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry
+        const float baseFrequency = pt1FilterApply(&motorFrequencyFilter[i], escState->rpm * erpmToHz); //Filter motor frequency
+
+        if (i < 4) {
+            DEBUG_SET(DEBUG_RPM_FREQ, i, (int)baseFrequency);
+        }
+
+        rpmGyroUpdateFn(&gyroRpmFilters, i, baseFrequency);
+        rpmDtermUpdateFn(&dtermRpmFilters, i, baseFrequency);
+    }
+}
+
+float FAST_CODE rpmFilterGyroApply(uint8_t axis, float input)
+{
+    return rpmGyroApplyFn(&gyroRpmFilters, axis, input);
+}
+
+float FAST_CODE rpmFilterDtermApply(uint8_t axis, float input)
+{
+    return rpmDtermApplyFn(&dtermRpmFilters, axis, input);
+}
+
+#endif

src/main/flight/rpm_filter.h

@@ -0,0 +1,53 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it 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.
+ *
+ * This file is distributed in the hope that it 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 program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include "config/parameter_group.h"
+#include "common/time.h"
+
+typedef struct rpmFilterConfig_s {
+    uint8_t gyro_filter_enabled;
+    uint8_t dterm_filter_enabled;
+
+    uint8_t  gyro_harmonics;
+    uint8_t  gyro_min_hz;
+    uint16_t gyro_q;
+
+    uint8_t  dterm_harmonics;
+    uint8_t  dterm_min_hz;
+    uint16_t dterm_q;
+
+} rpmFilterConfig_t;
+
+PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);
+
+#define RPM_FILTER_UPDATE_RATE_HZ 500
+#define RPM_FILTER_UPDATE_RATE_US (1000000.0f / RPM_FILTER_UPDATE_RATE_HZ)
+
+void disableRpmFilters(void);
+void rpmFiltersInit(void);
+void rpmFilterUpdateTask(timeUs_t currentTimeUs);
+float rpmFilterGyroApply(uint8_t axis, float input);
+float rpmFilterDtermApply(uint8_t axis, float input);

src/main/scheduler/scheduler.h

@@ -115,6 +115,9 @@ typedef enum {
 #endif
 #ifdef USE_GLOBAL_FUNCTIONS
     TASK_GLOBAL_FUNCTIONS,
+#endif
+#ifdef USE_RPM_FILTER
+    TASK_RPM_FILTER,
 #endif
     /* Count of real tasks */
     TASK_COUNT,

src/main/sensors/esc_sensor.c

@@ -130,6 +130,15 @@ static bool escSensorDecodeFrame(void)
     return ESC_SENSOR_FRAME_PENDING;
 }
 
+uint32_t FAST_CODE computeRpm(int16_t erpm) {
+    return lrintf((float)erpm * ERPM_PER_LSB / (motorConfig()->motorPoleCount / 2));
+}
+
+escSensorData_t NOINLINE * getEscTelemetry(uint8_t esc)
+{
+    return &escSensorData[esc];
+}
+
 escSensorData_t * escSensorGetData(void)
 {
     if (!escSensorPort) {
@@ -160,7 +169,7 @@ escSensorData_t * escSensorGetData(void)
         if (usedEscSensorCount) {
             escSensorDataCombined.current = (uint32_t)escSensorDataCombined.current * getMotorCount() / usedEscSensorCount + escSensorConfig()->currentOffset;
             escSensorDataCombined.voltage = (uint32_t)escSensorDataCombined.voltage / usedEscSensorCount;
-            escSensorDataCombined.rpm = lrintf(((float)escSensorDataCombined.rpm / usedEscSensorCount) * 100.0f / (motorConfig()->motorPoleCount / 2));
+            escSensorDataCombined.rpm = computeRpm((float)escSensorDataCombined.rpm / usedEscSensorCount);
         }
         else {
             escSensorDataCombined.dataAge = ESC_DATA_INVALID;

src/main/sensors/esc_sensor.h

@@ -40,7 +40,10 @@ PG_DECLARE(escSensorConfig_t, escSensorConfig);
 
 #define ESC_DATA_MAX_AGE    10
 #define ESC_DATA_INVALID    255
+#define ERPM_PER_LSB        100.0f
 
 bool escSensorInitialize(void);
 void escSensorUpdate(timeUs_t currentTimeUs);
 escSensorData_t * escSensorGetData(void);
+escSensorData_t * getEscTelemetry(uint8_t esc);
+uint32_t computeRpm(int16_t erpm);

src/main/sensors/gyro.c

@@ -68,6 +68,7 @@
 #include "sensors/sensors.h"
 
 #include "flight/gyroanalyse.h"
+#include "flight/rpm_filter.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
@@ -464,6 +465,12 @@ void FAST_CODE NOINLINE gyroUpdate()
 
         DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
 
+#ifdef USE_RPM_FILTER
+        DEBUG_SET(DEBUG_RPM_FILTER, axis, gyroADCf);
+        gyroADCf = rpmFilterGyroApply(axis, gyroADCf);
+        DEBUG_SET(DEBUG_RPM_FILTER, axis + 3, gyroADCf);
+#endif
+
         gyroADCf = gyroFilterStage2ApplyFn(stage2Filter[axis], gyroADCf);
         gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
         gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
@@ -484,6 +491,7 @@ void FAST_CODE NOINLINE gyroUpdate()
         gyroDataAnalyse(&gyroAnalyseState, notchFilterDyn, notchFilterDyn2);
     }
 #endif
+
 }
 
 bool gyroReadTemperature(void)

src/main/target/MATEKF722SE/target.h

@@ -198,4 +198,4 @@
 #define MAX_PWM_OUTPUT_PORTS        8
 #define USE_DSHOT
 #define USE_SERIALSHOT
-#define USE_ESC_SENSOR
+#define USE_ESC_SENSOR

src/main/target/OMNIBUS/target.h

@@ -154,5 +154,3 @@
 #define TARGET_IO_PORTB         0xffff
 #define TARGET_IO_PORTC         (BIT(13)|BIT(14)|BIT(15))
 #define TARGET_IO_PORTF         (BIT(0)|BIT(1)|BIT(4))
-
-#undef USE_PWM_SERVO_DRIVER

src/main/target/SPRACINGF3NEO/target.h

@@ -113,7 +113,6 @@
 #undef USE_VTX_FFPV
 #undef USE_VTX_SMARTAUDIO           // Disabled due to flash size
 #undef USE_VTX_TRAMP                // Disabled due to flash size
-#undef USE_PWM_SERVO_DRIVER         // Disabled due to RAM size
 
 #undef USE_PITOT                    // Disabled due to RAM size
 #undef USE_PITOT_ADC                // Disabled due to RAM size

src/main/target/common_post.h

@@ -40,6 +40,10 @@
 #define USE_CANVAS
 #endif
 
+#ifdef USE_ESC_SENSOR
+    #define USE_RPM_FILTER
+#endif
+
 #ifdef USE_ITCM_RAM
 #define FAST_CODE                   __attribute__((section(".tcm_code")))
 #define NOINLINE                    __NOINLINE
@@ -54,6 +58,7 @@
 #undef USE_SERIALRX_SUMH
 #undef USE_SERIALRX_XBUS
 #undef USE_SERIALRX_JETIEXBUS
+#undef USE_PWM_SERVO_DRIVER
 #endif
 
 #if defined(SIMULATOR_BUILD) || defined(UNIT_TEST)