Make filter type selectable and add BIQUAD

Adds options to select the filter type for both input and derivative.
rc_smoothing_input_type = PT1 | BIQUAD (default is BIQUAD)
rc_smoothing_derivative_type = OFF | PT1 | BIQUAD (default is OFF)

src/main/fc/fc_rc.c

@@ -250,7 +250,8 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
     uint8_t updatedChannel = 0;
 
     static float lastRxData[4];
-    static pt1Filter_t rcCommandFilter[4];
+    static pt1Filter_t rcCommandFilterPt1[4];
+    static biquadFilter_t rcCommandFilterBiquad[4];
     static bool initialized = false;
     static bool filterInitialized = false;
     static float rxFrameTimeSum;
@@ -285,9 +286,17 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
 
                     const float dT = targetPidLooptime * 1e-6f;
                     for (int i = 0; i < interpolationChannels; i++) {
-                        pt1FilterInit(&rcCommandFilter[i], pt1FilterGain(filterCutoffFrequency, dT));
+                        switch (rxConfig()->rc_smoothing_input_type) {
+                            case RC_SMOOTHING_INPUT_BIQUAD:
+                                biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime);
+                                break;
+                            case RC_SMOOTHING_INPUT_PT1:
+                            default:
+                                pt1FilterInit(&rcCommandFilterPt1[i], pt1FilterGain(filterCutoffFrequency, dT));
+                                break;
+                        }
                     }
-                    pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis);
+                    pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type);
                     filterInitialized = true;
                 }
             } else {
@@ -302,7 +311,15 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
 
     for (updatedChannel = ROLL; updatedChannel < interpolationChannels; updatedChannel++) {
         if (filterInitialized) {
-            rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilter[updatedChannel], lastRxData[updatedChannel]);
+            switch (rxConfig()->rc_smoothing_input_type) {
+                case RC_SMOOTHING_INPUT_BIQUAD:
+                    rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]);
+                    break;
+                case RC_SMOOTHING_INPUT_PT1:
+                default:
+                    rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilterPt1[updatedChannel], lastRxData[updatedChannel]);
+                    break;
+            }
         } else {
             // If filter isn't initialized yet then use the actual unsmoothed rx channel data
             rcCommand[updatedChannel] = lastRxData[updatedChannel];

src/main/fc/rc_controls.h

@@ -61,7 +61,18 @@ typedef enum {
 typedef enum {
     RC_SMOOTHING_TYPE_INTERPOLATION,
     RC_SMOOTHING_TYPE_FILTER
-} rcInterpolationType_e;
+} rcSmoothingType_e;
+
+typedef enum {
+    RC_SMOOTHING_INPUT_PT1,
+    RC_SMOOTHING_INPUT_BIQUAD
+} rcSmoothingInputFilter_e;
+
+typedef enum {
+    RC_SMOOTHING_DERIVATIVE_OFF,
+    RC_SMOOTHING_DERIVATIVE_PT1,
+    RC_SMOOTHING_DERIVATIVE_BIQUAD
+} rcSmoothingDerivativeFilter_e;
 
 
 #define ROL_LO (1 << (2 * ROLL))

src/main/flight/pid.c

@@ -211,9 +211,11 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh;
 #endif
 
 #ifdef USE_RC_SMOOTHING_FILTER
-static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativeLpf[2];
+static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[2];
+static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[2];
 static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized;
 static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
+static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
 #endif // USE_RC_SMOOTHING_FILTER
 
 void pidInitFilters(const pidProfile_t *pidProfile)
@@ -302,11 +304,19 @@ void pidInitFilters(const pidProfile_t *pidProfile)
 }
 
 #ifdef USE_RC_SMOOTHING_FILTER
-void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis) {
+void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType) {
     setpointDerivativeLpfInitialized = true;
     rcSmoothingDebugAxis = debugAxis;
+    rcSmoothingFilterType = filterType;
     for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
-        pt1FilterInit(&setpointDerivativeLpf[axis], pt1FilterGain(filterCutoff, dT));
+        switch (rcSmoothingFilterType) {
+            case RC_SMOOTHING_DERIVATIVE_PT1:
+                pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
+                break;
+            case RC_SMOOTHING_DERIVATIVE_BIQUAD:
+                biquadFilterInitLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
+                break;
+        }
     }
 }
 #endif // USE_RC_SMOOTHING_FILTER
@@ -884,9 +894,15 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
             if (axis == rcSmoothingDebugAxis) {
                 DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
             }
-            if ((dynCd != 0) && setpointDerivativeLpfInitialized) {
+            if ((dynCd != 0) && setpointDerivativeLpfInitialized && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
-                pidSetpointDelta = pt1FilterApply(&setpointDerivativeLpf[axis], pidSetpointDelta);
+                switch (rcSmoothingFilterType) {
-
+                    case RC_SMOOTHING_DERIVATIVE_PT1:
+                        pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
+                        break;
+                    case RC_SMOOTHING_DERIVATIVE_BIQUAD:
+                        pidSetpointDelta = biquadFilterApply(&setpointDerivativeBiquad[axis], pidSetpointDelta);
+                        break;
+                }
                 if (axis == rcSmoothingDebugAxis) {
                     DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(pidSetpointDelta * 100.0f));
                 }

src/main/flight/pid.h

@@ -178,4 +178,4 @@ void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
 bool crashRecoveryModeActive(void);
 void pidAcroTrainerInit(void);
 void pidSetAcroTrainerState(bool newState);
-void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis);
+void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);

src/main/interface/settings.c

@@ -356,6 +356,12 @@ static const char * const lookupTableRcSmoothingType[] = {
 static const char * const lookupTableRcSmoothingDebug[] = {
     "ROLL", "PITCH", "YAW", "THROTTLE"
 };
+static const char * const lookupTableRcSmoothingInputType[] = {
+    "PT1", "BIQUAD"
+};
+static const char * const lookupTableRcSmoothingDerivativeType[] = {
+    "OFF", "PT1", "BIQUAD"
+};
 #endif // USE_RC_SMOOTHING_FILTER
 
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
@@ -442,6 +448,8 @@ const lookupTableEntry_t lookupTables[] = {
 #ifdef USE_RC_SMOOTHING_FILTER
     LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingType),
     LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDebug),
+    LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType),
+    LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
 #endif // USE_RC_SMOOTHING_FILTER
 };
 
@@ -544,6 +552,8 @@ const clivalue_t valueTable[] = {
     { "rc_smoothing_input_hz",      VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_cutoff) },
     { "rc_smoothing_derivative_hz", VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_cutoff) },
     { "rc_smoothing_debug_axis",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DEBUG }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_debug_axis) },
+    { "rc_smoothing_input_type",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_INPUT_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_type) },
+    { "rc_smoothing_derivative_type",VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DERIVATIVE_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_type) },
 #endif // USE_RC_SMOOTHING_FILTER
 
     { "fpv_mix_degrees",            VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) },

src/main/interface/settings.h

@@ -107,6 +107,8 @@ typedef enum {
 #ifdef USE_RC_SMOOTHING_FILTER
     TABLE_RC_SMOOTHING_TYPE,
     TABLE_RC_SMOOTHING_DEBUG,
+    TABLE_RC_SMOOTHING_INPUT_TYPE,
+    TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
 #endif // USE_RC_SMOOTHING_FILTER
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;

src/main/pg/rx.c

@@ -64,6 +64,8 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
         .rc_smoothing_input_cutoff = 0,      // automatically calculate the cutoff by default
         .rc_smoothing_derivative_cutoff = 0, // automatically calculate the cutoff by default
         .rc_smoothing_debug_axis = ROLL,     // default to debug logging for the roll axis
+        .rc_smoothing_input_type = RC_SMOOTHING_INPUT_BIQUAD,
+        .rc_smoothing_derivative_type = RC_SMOOTHING_DERIVATIVE_OFF,
     );
 
 #ifdef RX_CHANNELS_TAER

src/main/pg/rx.h

@@ -55,6 +55,8 @@ typedef struct rxConfig_s {
     uint8_t rc_smoothing_input_cutoff;      // Filter cutoff frequency for the input filter (0 = auto)
     uint8_t rc_smoothing_derivative_cutoff; // Filter cutoff frequency for the setpoint weight derivative filter (0 = auto)
     uint8_t rc_smoothing_debug_axis;        // Axis to log as debug values when debug_mode = RC_SMOOTHING
+    uint8_t rc_smoothing_input_type;        // Input filter type (0 = PT1, 1 = BIQUAD)
+    uint8_t rc_smoothing_derivative_type;   // Derivative filter type (0 = OFF, 1 = PT1, 2 = BIQUAD)
 } rxConfig_t;
 
 PG_DECLARE(rxConfig_t, rxConfig);