RC smoothing: improve rx frame rate detection, add rc_smoothing_info cli command

Improved the rx frame rate detection/training by delaying calculation to avoid loop time jitter during flight controller initialization.

For auto cutoffs calculate a value appropriate for BIQUAD or PT1 depending on the configuration.

Added a new rc_smoothing_info cli command to display internal details about its operation.

src/main/fc/fc_rc.c

@@ -69,11 +69,17 @@ enum {
 };
 
 #ifdef USE_RC_SMOOTHING_FILTER
-#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 3000 // Wait 3 seconds after power to let the PID loop stabilize before starting average frame rate calculation
+#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 5000 // Wait 5 seconds after power to let the PID loop stabilize before starting average frame rate calculation
 #define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50
 #define RC_SMOOTHING_FILTER_INPUT_AUTO_HZ      50.0f // Used to calculate the default cutoff based on rx frame rate. For example, 9ms frame should use 50hz
 #define RC_SMOOTHING_FILTER_DERIVATIVE_AUTO_HZ 60.0f // Used to calculate the derivative default cutoff based on rx frame rate. For example, 9ms frame should use 60hz
 #define RC_SMOOTHING_FILTER_AUTO_MS 9.0f  // Formula used: RC_SMOOTHING_FILTER_AUTO_HZ / (measured rx frame delay / RC_SMOOTHING_FILTER_AUTO_HZ)
+
+static FAST_RAM_ZERO_INIT uint16_t defaultInputCutoffFrequency;
+static FAST_RAM_ZERO_INIT uint16_t defaultDerivativeCutoffFrequency;
+static FAST_RAM_ZERO_INIT uint16_t filterCutoffFrequency;
+static FAST_RAM_ZERO_INIT uint16_t derivativeCutoffFrequency;
+static FAST_RAM_ZERO_INIT uint16_t calculatedFrameAverageMs;
 #endif // USE_RC_SMOOTHING_FILTER
 
 float getSetpointRate(int axis)
@@ -259,9 +265,14 @@ FAST_CODE uint8_t processRcInterpolation(void)
 }
 
 #ifdef USE_RC_SMOOTHING_FILTER
-uint8_t calcRcSmoothingCutoff(float avgRxFrameRate, float filterBaseline)
+uint16_t calcRcSmoothingCutoff(float avgRxFrameRate, float filterBaseline, bool pt1)
 {
-    return lrintf(filterBaseline / (avgRxFrameRate / RC_SMOOTHING_FILTER_AUTO_MS));
+    float cutoff = filterBaseline / (avgRxFrameRate / RC_SMOOTHING_FILTER_AUTO_MS);
+    if (pt1) { // Convert to a PT1 cutoff (2pi * cutoff)^2 / 500 / 2pi;
+        const float twoPi = 2.0f * M_PIf;
+        cutoff = (sq(cutoff * twoPi) / 500) / twoPi;
+    }
+    return lrintf(cutoff);
 }
 
 FAST_CODE uint8_t processRcSmoothingFilter(void)
@@ -275,10 +286,8 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
     static FAST_RAM_ZERO_INIT bool filterInitialized;
     static FAST_RAM_ZERO_INIT float rxFrameTimeSum;
     static FAST_RAM_ZERO_INIT int rxFrameCount;
-    static FAST_RAM_ZERO_INIT uint16_t defaultInputCutoffFrequency;
+    static FAST_RAM uint16_t minRxFrameInterval = UINT16_MAX;
-    static FAST_RAM_ZERO_INIT uint16_t defaultDerivativeCutoffFrequency;
+    static FAST_RAM_ZERO_INIT uint16_t maxRxFrameInterval;
-    static FAST_RAM_ZERO_INIT uint16_t filterCutoffFrequency;
-    static FAST_RAM_ZERO_INIT uint16_t derivativeCutoffFrequency;
 
     if (!initialized) {
         initialized = true;
@@ -298,24 +307,36 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
             if (rxIsReceivingSignal() && (targetPidLooptime > 0) && (millis() > RC_SMOOTHING_FILTER_TRAINING_DELAY_MS)) {
                 rxFrameTimeSum += currentRxRefreshRate;
                 rxFrameCount++;
+                maxRxFrameInterval = MAX(maxRxFrameInterval, currentRxRefreshRate);
+                minRxFrameInterval = MIN(minRxFrameInterval, currentRxRefreshRate);
+                DEBUG_SET(DEBUG_RC_SMOOTHING, 0, rxFrameCount);           // log the step count during training
+                DEBUG_SET(DEBUG_RC_SMOOTHING, 3, currentRxRefreshRate);   // log each frame interval during training
                 if (rxFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) {
-                    const float avgRxFrameRate = rxFrameTimeSum / rxFrameCount / 1000;
+                    rxFrameTimeSum = rxFrameTimeSum - minRxFrameInterval - maxRxFrameInterval; // Throw out high and low samples
-                    defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_INPUT_AUTO_HZ);
+                    calculatedFrameAverageMs = lrintf(rxFrameTimeSum / (rxFrameCount - 2));
-                    defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_DERIVATIVE_AUTO_HZ);
+                    const float avgRxFrameRate = (rxFrameTimeSum / (rxFrameCount - 2)) / 1000;
+
+                    defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_INPUT_AUTO_HZ, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1));
                     filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultInputCutoffFrequency : filterCutoffFrequency;
+
+                    if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) {
+                        derivativeCutoffFrequency = 0;
+                    } else {
+                        defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameRate, RC_SMOOTHING_FILTER_DERIVATIVE_AUTO_HZ, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1));
                         derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency;
+                    }
 
                     const float dT = targetPidLooptime * 1e-6f;
                     for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
                         if ((1 << i) & interpolationChannels) {
                             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;
+                                case RC_SMOOTHING_INPUT_BIQUAD:
+                                default:
+                                    biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime);
+                                    break;
                             }
                         }
                     }
@@ -329,20 +350,24 @@ FAST_CODE uint8_t processRcSmoothingFilter(void)
         }
     }
 
+    if (filterInitialized && (debugMode = DEBUG_RC_SMOOTHING)) {
+        // after training has completed then log the raw rc channel and the calculated
+        // average rx frame rate that was used to calculate the automatic filter cutoffs
         DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis]));
-    DEBUG_SET(DEBUG_RC_SMOOTHING, 3, defaultInputCutoffFrequency);
+        DEBUG_SET(DEBUG_RC_SMOOTHING, 3, calculatedFrameAverageMs);
+    }
 
     for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
         if ((1 << updatedChannel) & interpolationChannels) {
             if (filterInitialized) {
                 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;
+                    case RC_SMOOTHING_INPUT_BIQUAD:
+                    default:
+                        rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]);
+                        break;
                 }
             } else {
                 // If filter isn't initialized yet then use the actual unsmoothed rx channel data
@@ -557,3 +582,23 @@ void initRcProcessing(void)
         break;
     }
 }
+
+#ifdef USE_RC_SMOOTHING_FILTER
+uint16_t rcSmoothingGetValue(int whichValue)
+{
+    switch (whichValue) {
+        case RC_SMOOTHING_VALUE_INPUT_AUTO:
+            return defaultInputCutoffFrequency;
+        case RC_SMOOTHING_VALUE_INPUT_ACTIVE:
+            return filterCutoffFrequency;
+        case RC_SMOOTHING_VALUE_DERIVATIVE_AUTO:
+            return defaultDerivativeCutoffFrequency;
+        case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE:
+            return derivativeCutoffFrequency;
+        case RC_SMOOTHING_VALUE_AVERAGE_FRAME:
+            return calculatedFrameAverageMs;
+        default:
+            return 0;
+    }
+}
+#endif // USE_RC_SMOOTHING_FILTER

src/main/fc/fc_rc.h

@@ -39,3 +39,4 @@ void updateRcCommands(void);
 void resetYawAxis(void);
 void initRcProcessing(void);
 bool isMotorsReversed(void);
+uint16_t rcSmoothingGetValue(int whichValue);

src/main/fc/rc_controls.h

@@ -76,6 +76,13 @@ typedef enum {
     RC_SMOOTHING_DERIVATIVE_BIQUAD
 } rcSmoothingDerivativeFilter_e;
 
+typedef enum {
+    RC_SMOOTHING_VALUE_INPUT_AUTO,
+    RC_SMOOTHING_VALUE_INPUT_ACTIVE,
+    RC_SMOOTHING_VALUE_DERIVATIVE_AUTO,
+    RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE,
+    RC_SMOOTHING_VALUE_AVERAGE_FRAME
+} rcSmoothingInfoType_e;
 
 #define ROL_LO (1 << (2 * ROLL))
 #define ROL_CE (3 << (2 * ROLL))

src/main/flight/pid.c

@@ -306,10 +306,12 @@ void pidInitFilters(const pidProfile_t *pidProfile)
 }
 
 #ifdef USE_RC_SMOOTHING_FILTER
-void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType) {
+void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType)
-    setpointDerivativeLpfInitialized = true;
+{
     rcSmoothingDebugAxis = debugAxis;
     rcSmoothingFilterType = filterType;
+    if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
+        setpointDerivativeLpfInitialized = true;
         for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
             switch (rcSmoothingFilterType) {
                 case RC_SMOOTHING_DERIVATIVE_PT1:
@@ -320,6 +322,7 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
                     break;
             }
         }
+    }
 }
 #endif // USE_RC_SMOOTHING_FILTER
 
@@ -896,7 +899,7 @@ 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 && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
+            if ((dynCd != 0) && setpointDerivativeLpfInitialized) {
                 switch (rcSmoothingFilterType) {
                     case RC_SMOOTHING_DERIVATIVE_PT1:
                         pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);

src/main/interface/cli.c

@@ -3654,6 +3654,44 @@ static void cliVersion(char *cmdline)
     );
 }
 
+#ifdef USE_RC_SMOOTHING_FILTER
+static void cliRcSmoothing(char *cmdline)
+{
+    UNUSED(cmdline);
+    cliPrint("# RC Smoothing Type: ");
+    if (rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_FILTER) {
+        cliPrintLine("FILTER");
+        uint16_t avgRxFrameMs = rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME);
+        cliPrint("# Detected RX frame rate: ");
+        if (avgRxFrameMs == 0) {
+            cliPrintLine("NO SIGNAL");
+        } else {
+            cliPrintLinef("%d.%dms", avgRxFrameMs / 1000, avgRxFrameMs % 1000);
+        }
+        cliPrintLinef("# Auto input cutoff: %dhz", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_AUTO));
+        cliPrintf("# Active input cutoff: %dhz ", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE));
+        if (rxConfig()->rc_smoothing_input_cutoff == 0) {
+            cliPrintLine("(auto)");
+        } else {
+            cliPrintLine("(manual)");
+        }
+        cliPrintLinef("# Auto derivative cutoff: %dhz", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_AUTO));
+        cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE));
+        if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) {
+            cliPrintLine("off)");
+        } else {
+            if (rxConfig()->rc_smoothing_derivative_cutoff == 0) {
+                cliPrintLine("auto)");
+            } else {
+                cliPrintLine("manual)");
+            }
+        }
+    } else {
+        cliPrintLine("INTERPOLATION");
+    }
+}
+#endif // USE_RC_SMOOTHING_FILTER
+
 #if defined(USE_RESOURCE_MGMT)
 
 #define MAX_RESOURCE_INDEX(x) ((x) == 0 ? 1 : (x))
@@ -4441,6 +4479,9 @@ const clicmd_t cmdTable[] = {
 #endif
     CLI_COMMAND_DEF("profile", "change profile", "[<index>]", cliProfile),
     CLI_COMMAND_DEF("rateprofile", "change rate profile", "[<index>]", cliRateProfile),
+#ifdef USE_RC_SMOOTHING_FILTER
+    CLI_COMMAND_DEF("rc_smoothing_info", "show rc_smoothing operational settings", NULL, cliRcSmoothing),
+#endif // USE_RC_SMOOTHING_FILTER
 #ifdef USE_RESOURCE_MGMT
     CLI_COMMAND_DEF("resource", "show/set resources", NULL, cliResource),
 #endif