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);