Merge pull request #6194 from etracer65/rc_smoothing_retraining

RC smoothing retraining update - adds full support for CRSF

src/main/blackbox/blackbox.c

@@ -1333,6 +1333,7 @@ static bool blackboxWriteSysinfo(void)
                                                                             rxConfig()->rc_smoothing_derivative_type);
         BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE),
                                                                             rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE));
+        BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d",         rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME));
 #endif // USE_RC_SMOOTHING_FILTER
 
 

src/main/build/debug.c

@@ -73,4 +73,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "ACRO_TRAINER",
     "RC_SMOOTHING",
     "RX_SIGNAL_LOSS",
+    "RC_SMOOTHING_RATE",
 };

src/main/build/debug.h

@@ -91,6 +91,7 @@ typedef enum {
     DEBUG_ACRO_TRAINER,
     DEBUG_RC_SMOOTHING,
     DEBUG_RX_SIGNAL_LOSS,
+    DEBUG_RC_SMOOTHING_RATE,
     DEBUG_COUNT
 } debugType_e;
 

src/main/common/filter.c

@@ -56,6 +56,11 @@ void pt1FilterInit(pt1Filter_t *filter, float k)
     filter->k = k;
 }
 
+void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k)
+{
+    filter->k = k;
+}
+
 FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
 {
     filter->state = filter->state + filter->k * (input - filter->state);
@@ -167,6 +172,11 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
     filter->y2 = y2;
 }
 
+FAST_CODE void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
+{
+    biquadFilterUpdate(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
+}
+
 /* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
 FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
 {

src/main/common/filter.h

@@ -67,6 +67,7 @@ float nullFilterApply(filter_t *filter, float input);
 void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
 void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
+void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
 
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
 float biquadFilterApply(biquadFilter_t *filter, float input);
@@ -77,6 +78,7 @@ float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);
 
 float pt1FilterGain(uint16_t f_cut, float dT);
 void pt1FilterInit(pt1Filter_t *filter, float k);
+void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
 float pt1FilterApply(pt1Filter_t *filter, float input);
 
 void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);

src/main/fc/fc_rc.c

@@ -68,16 +68,16 @@ enum {
 };
 
 #ifdef USE_RC_SMOOTHING_FILTER
-#define RC_SMOOTHING_IDENTITY_FREQUENCY 80         // Used in the formula to convert a BIQUAD cutoff frequency to PT1
+#define RC_SMOOTHING_IDENTITY_FREQUENCY         80    // Used in the formula to convert a BIQUAD cutoff frequency to PT1
-#define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS 5000  // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation
+#define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS    5000  // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation
-#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 1000 // Additional time to wait after receiving first valid rx frame before training starts
+#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES    50    // Number of rx frame rate samples to average
-#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50
+#define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS   1000  // Additional time to wait after receiving first valid rx frame before initial training starts
+#define RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS 2000  // Guard time to wait after retraining to prevent retraining again too quickly
+#define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT     20    // Look for samples varying this much from the current detected frame rate to initiate retraining
+#define RC_SMOOTHING_RX_RATE_MIN_US             5000  // 5ms or 200hz
+#define RC_SMOOTHING_RX_RATE_MAX_US             50000 // 50ms or 20hz
 
-static FAST_RAM_ZERO_INIT uint16_t defaultInputCutoffFrequency;
+static FAST_RAM_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
-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 calculatedFrameTimeAverageUs;
 #endif // USE_RC_SMOOTHING_FILTER
 
 float getSetpointRate(int axis)
@@ -263,11 +263,14 @@ FAST_CODE uint8_t processRcInterpolation(void)
 }
 
 #ifdef USE_RC_SMOOTHING_FILTER
-int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1)
+// Determine a cutoff frequency based on filter type and the calculated
+// average rx frame time
+FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1)
 {
-    if (avgRxFrameTime > 0) {
+    if (avgRxFrameTimeUs > 0) {
-        float cutoff = (1 / avgRxFrameTime) / 2;  // calculate the nyquist frequency
+        float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 2;  // calculate the nyquist frequency
         cutoff = cutoff * 0.90f;  // Use 90% of the calculated nyquist frequency
+
         if (pt1) {
             cutoff = sq(cutoff) / RC_SMOOTHING_IDENTITY_FREQUENCY; // convert to a cutoff for pt1 that has similar characteristics
         }
@@ -277,107 +280,231 @@ int calcRcSmoothingCutoff(float avgRxFrameTime, bool pt1)
     }
 }
 
+// Preforms a reasonableness check on the rx frame time to avoid bad data
+// skewing the average.
+FAST_CODE bool rcSmoothingRxRateValid(int currentRxRefreshRate)
+{
+    return (currentRxRefreshRate >= RC_SMOOTHING_RX_RATE_MIN_US && currentRxRefreshRate <= RC_SMOOTHING_RX_RATE_MAX_US);
+}
+
+// Initialize or update the filters base on either the manually selected cutoff, or
+// the auto-calculated cutoff frequency based on detected rx frame rate.
+FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothingData)
+{
+    const float dT = targetPidLooptime * 1e-6f;
+    uint16_t oldCutoff = smoothingData->inputCutoffFrequency;
+    
+    if (rxConfig()->rc_smoothing_input_cutoff == 0) {
+        smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1));
+    }
+
+    // initialize or update the input filter
+    if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
+        for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
+            if ((1 << i) & interpolationChannels) {  // only update channels specified by rc_interp_ch
+                switch (rxConfig()->rc_smoothing_input_type) {
+                
+                    case RC_SMOOTHING_INPUT_PT1:
+                        if (!smoothingData->filterInitialized) {
+                            pt1FilterInit((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT));
+                        } else {
+                            pt1FilterUpdateCutoff((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT));
+                        }
+                        break;
+                        
+                    case RC_SMOOTHING_INPUT_BIQUAD:
+                    default:
+                        if (!smoothingData->filterInitialized) {
+                            biquadFilterInitLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime);
+                        } else {
+                            biquadFilterUpdateLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime);
+                        }
+                        break;
+                }
+            }
+        }
+    }
+
+    // update or initialize the derivative filter
+    oldCutoff = smoothingData->derivativeCutoffFrequency;
+    if ((rxConfig()->rc_smoothing_derivative_cutoff == 0) && (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF)) {
+        smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1));
+    }
+
+    if (!smoothingData->filterInitialized) {
+        pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type);
+    } else if (smoothingData->derivativeCutoffFrequency != oldCutoff) {
+        pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency);
+    }
+}
+
+FAST_CODE_NOINLINE void rcSmoothingResetAccumulation(rcSmoothingFilter_t *smoothingData)
+{
+    smoothingData->training.sum = 0;
+    smoothingData->training.count = 0;
+    smoothingData->training.min = UINT16_MAX;
+    smoothingData->training.max = 0;
+}
+
+// Accumulate the rx frame time samples. Once we've collected enough samples calculate the
+// average and return true.
+FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothingData, int rxFrameTimeUs)
+{
+    smoothingData->training.sum += rxFrameTimeUs;
+    smoothingData->training.count++;
+    smoothingData->training.max = MAX(smoothingData->training.max, rxFrameTimeUs);
+    smoothingData->training.min = MIN(smoothingData->training.min, rxFrameTimeUs);
+
+    // if we've collected enough samples then calculate the average and reset the accumulation
+    if (smoothingData->training.count >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) {
+        smoothingData->training.sum = smoothingData->training.sum - smoothingData->training.min - smoothingData->training.max; // Throw out high and low samples
+        smoothingData->averageFrameTimeUs = lrintf(smoothingData->training.sum / (smoothingData->training.count - 2));
+        rcSmoothingResetAccumulation(smoothingData);
+        return true;
+    }
+    return false;
+}
+
+// Determine if we need to caclulate filter cutoffs. If not then we can avoid
+// examining the rx frame times completely 
+FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
+{
+    bool ret = false;
+
+    // if the input cutoff is 0 (auto) then we need to calculate cutoffs
+    if (rxConfig()->rc_smoothing_input_cutoff == 0) {
+        ret = true;
+    }
+
+    // if the derivative type isn't OFF and the cutoff is 0 then we need to calculate
+    if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) {
+        if (rxConfig()->rc_smoothing_derivative_cutoff == 0) {
+            ret = true;
+        }
+    }
+    return ret;
+}
+
 FAST_CODE uint8_t processRcSmoothingFilter(void)
 {
     uint8_t updatedChannel = 0;
-
     static FAST_RAM_ZERO_INIT float lastRxData[4];
-    static FAST_RAM_ZERO_INIT pt1Filter_t rcCommandFilterPt1[4];
-    static FAST_RAM_ZERO_INIT biquadFilter_t rcCommandFilterBiquad[4];
     static FAST_RAM_ZERO_INIT bool initialized;
-    static FAST_RAM_ZERO_INIT bool filterInitialized;
-    static FAST_RAM_ZERO_INIT float rxFrameTimeSum;
-    static FAST_RAM_ZERO_INIT int rxFrameCount;
-    static FAST_RAM uint16_t minRxFrameInterval = UINT16_MAX;
-    static FAST_RAM_ZERO_INIT uint16_t maxRxFrameInterval;
     static FAST_RAM_ZERO_INIT timeMs_t validRxFrameTimeMs;
+    static FAST_RAM_ZERO_INIT bool calculateCutoffs;
 
+    // first call initialization
     if (!initialized) {
         initialized = true;
-        filterCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff;
+        rcSmoothingData.filterInitialized = false;
-        derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff;
+        rcSmoothingData.averageFrameTimeUs = 0;
+        rcSmoothingResetAccumulation(&rcSmoothingData);
+        
+        rcSmoothingData.inputCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff;
+        
+        if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) {
+            rcSmoothingData.derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff;
+        }
+        
+        calculateCutoffs = rcSmoothingAutoCalculate();
+
+        // if we don't need to calculate cutoffs dynamically then the filters can be initialized now
+        if (!calculateCutoffs) {
+            rcSmoothingSetFilterCutoffs(&rcSmoothingData);
+            rcSmoothingData.filterInitialized = true;
+        }
     }
 
     if (isRXDataNew) {
+
+        // store the new raw channel values
         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
             if ((1 << i) & interpolationChannels) {
                 lastRxData[i] = rcCommand[i];
             }
         }
-        // If the filter cutoffs are set to auto and we have good rx data, then determine the average rx frame rate
+
-        // and use that to calculate the filter cutoff frequencies
+        // for dynamically calculated filters we need to examine each rx frame interval
-        if (!filterInitialized) {
+        if (calculateCutoffs) {
             const timeMs_t currentTimeMs = millis();
-            if (rxIsReceivingSignal() && (targetPidLooptime > 0) && (currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS)) {
+            int sampleState = 0;
-                if (validRxFrameTimeMs == 0) {
-                    validRxFrameTimeMs = currentTimeMs;
-                } else if ((currentTimeMs - validRxFrameTimeMs) > 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) {
-                        rxFrameTimeSum = rxFrameTimeSum - minRxFrameInterval - maxRxFrameInterval; // Throw out high and low samples
-                        calculatedFrameTimeAverageUs = lrintf(rxFrameTimeSum / (rxFrameCount - 2));
-                        const float avgRxFrameTime = (rxFrameTimeSum / (rxFrameCount - 2)) * 1e-6f;
 
-                        defaultInputCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1));
+            // If the filter cutoffs are set to auto and we have good rx data, then determine the average rx frame rate
-                        filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultInputCutoffFrequency : filterCutoffFrequency;
+            // and use that to calculate the filter cutoff frequencies
+            if ((currentTimeMs > RC_SMOOTHING_FILTER_STARTUP_DELAY_MS) && (targetPidLooptime > 0)) { // skip during FC initialization
+                if (rxIsReceivingSignal()  && rcSmoothingRxRateValid(currentRxRefreshRate)) {
 
-                        if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) {
+                    // set the guard time expiration if it's not set
-                            derivativeCutoffFrequency = 0;
+                    if (validRxFrameTimeMs == 0) {
-                        } else {
+                        validRxFrameTimeMs = currentTimeMs + (rcSmoothingData.filterInitialized ? RC_SMOOTHING_FILTER_RETRAINING_DELAY_MS : RC_SMOOTHING_FILTER_TRAINING_DELAY_MS);
-                            defaultDerivativeCutoffFrequency = calcRcSmoothingCutoff(avgRxFrameTime, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1));
+                    } else {
-                            derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency;
+                        sampleState = 1;
-                        }
+                    }
 
-                        const float dT = targetPidLooptime * 1e-6f;
+                    // if the guard time has expired then process the rx frame time
-                        for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
+                    if (currentTimeMs > validRxFrameTimeMs) {
-                            if ((1 << i) & interpolationChannels) {
+                        sampleState = 2;
-                                switch (rxConfig()->rc_smoothing_input_type) {
+                        bool accumulateSample = true;
-                                    case RC_SMOOTHING_INPUT_PT1:
+
-                                        pt1FilterInit(&rcCommandFilterPt1[i], pt1FilterGain(filterCutoffFrequency, dT));
+                        // During initial training process all samples.
-                                        break;
+                        // During retraining check samples to determine if they vary by more than the limit percentage.
-                                    case RC_SMOOTHING_INPUT_BIQUAD:
+                        if (rcSmoothingData.filterInitialized) {
-                                    default:
+                            const float percentChange = (ABS(currentRxRefreshRate - rcSmoothingData.averageFrameTimeUs) / (float)rcSmoothingData.averageFrameTimeUs) * 100;
-                                        biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime);
+                            if (percentChange < RC_SMOOTHING_RX_RATE_CHANGE_PERCENT) {
-                                        break;
+                                // We received a sample that wasn't more than the limit percent so reset the accumulation
-                                }
+                                // During retraining we need a contiguous block of samples that are all significantly different than the current average
+                                rcSmoothingResetAccumulation(&rcSmoothingData);
+                                accumulateSample = false;
                             }
                         }
-                        pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type);
+
-                        filterInitialized = true;
+                        // accumlate the sample into the average
+                        if (accumulateSample) {
+                            if (rcSmoothingAccumulateSample(&rcSmoothingData, currentRxRefreshRate)) {
+                                // the required number of samples were collected so set the filter cutoffs
+                                rcSmoothingSetFilterCutoffs(&rcSmoothingData);
+                                rcSmoothingData.filterInitialized = true;
+                                validRxFrameTimeMs = 0;
+                            }
+                        }
+
                     }
+                } else {
+                    // we have either stopped receiving rx samples (failsafe?) or the sample time is unreasonable so reset the accumulation
+                    validRxFrameTimeMs = 0;
+                    rcSmoothingResetAccumulation(&rcSmoothingData);
                 }
-            } else {
+            }
-                rxFrameTimeSum = 0;
+
-                rxFrameCount = 0;
+            // rx frame rate training blackbox debugging
-                validRxFrameTimeMs = 0;
+            if (debugMode == DEBUG_RC_SMOOTHING_RATE) {
-                minRxFrameInterval = UINT16_MAX;
+                DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate);              // log each rx frame interval
-                maxRxFrameInterval = 0;
+                DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.training.count);    // log the training step count
+                DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.averageFrameTimeUs);// the current calculated average
+                DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState);                       // indicates whether guard time is active
             }
         }
     }
 
-    if (filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) {
+    if (rcSmoothingData.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, calculatedFrameTimeAverageUs);
+        DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs);
     }
 
+    // each pid loop continue to apply the last received channel value to the filter
     for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
-        if ((1 << updatedChannel) & interpolationChannels) {
+        if ((1 << updatedChannel) & interpolationChannels) {  // only smooth selected channels base on the rc_interp_ch value
-            if (filterInitialized) {
+            if (rcSmoothingData.filterInitialized) {
                 switch (rxConfig()->rc_smoothing_input_type) {
                     case RC_SMOOTHING_INPUT_PT1:
-                        rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilterPt1[updatedChannel], lastRxData[updatedChannel]);
+                        rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
                         break;
+
                     case RC_SMOOTHING_INPUT_BIQUAD:
                     default:
-                        rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]);
+                        rcCommand[updatedChannel] = biquadFilterApplyDF1((biquadFilter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
                         break;
                 }
             } else {
@@ -594,16 +721,12 @@ void initRcProcessing(void)
 int rcSmoothingGetValue(int whichValue)
 {
     switch (whichValue) {
-        case RC_SMOOTHING_VALUE_INPUT_AUTO:
-            return defaultInputCutoffFrequency;
         case RC_SMOOTHING_VALUE_INPUT_ACTIVE:
-            return filterCutoffFrequency;
+            return rcSmoothingData.inputCutoffFrequency;
-        case RC_SMOOTHING_VALUE_DERIVATIVE_AUTO:
-            return defaultDerivativeCutoffFrequency;
         case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE:
-            return derivativeCutoffFrequency;
+            return rcSmoothingData.derivativeCutoffFrequency;
         case RC_SMOOTHING_VALUE_AVERAGE_FRAME:
-            return calculatedFrameTimeAverageUs;
+            return rcSmoothingData.averageFrameTimeUs;
         default:
             return 0;
     }

src/main/fc/fc_rc.h

@@ -40,3 +40,4 @@ void resetYawAxis(void);
 void initRcProcessing(void);
 bool isMotorsReversed(void);
 int rcSmoothingGetValue(int whichValue);
+bool rcSmoothingAutoCalculate(void);

src/main/fc/rc_controls.h

@@ -22,6 +22,7 @@
 
 #include <stdbool.h>
 
+#include "common/filter.h"
 #include "pg/pg.h"
 
 typedef enum rc_alias {
@@ -77,9 +78,7 @@ typedef enum {
 } 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;
@@ -108,6 +107,27 @@ typedef enum {
 
 extern float rcCommand[4];
 
+typedef struct rcSmoothingFilterTraining_s {
+    float sum;
+    int count;
+    uint16_t min;
+    uint16_t max;
+} rcSmoothingFilterTraining_t;
+
+typedef union rcSmoothingFilterTypes_u {
+    pt1Filter_t pt1Filter;
+    biquadFilter_t biquadFilter;
+} rcSmoothingFilterTypes_t;
+
+typedef struct rcSmoothingFilter_s {
+    bool filterInitialized;
+    rcSmoothingFilterTypes_t filter[4];
+    uint16_t inputCutoffFrequency;
+    uint16_t derivativeCutoffFrequency;
+    int averageFrameTimeUs;
+    rcSmoothingFilterTraining_t training;
+} rcSmoothingFilter_t;
+
 typedef struct rcControlsConfig_s {
     uint8_t deadband;                       // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
     uint8_t yaw_deadband;                   // introduce a deadband around the stick center for yaw axis. Must be greater than zero.

src/main/flight/pid.c

@@ -325,6 +325,22 @@ void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint
         }
     }
 }
+
+void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
+{
+    if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
+        for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
+            switch (rcSmoothingFilterType) {
+                case RC_SMOOTHING_DERIVATIVE_PT1:
+                    pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
+                    break;
+                case RC_SMOOTHING_DERIVATIVE_BIQUAD:
+                    biquadFilterUpdateLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
+                    break;
+            }
+        }
+    }
+}
 #endif // USE_RC_SMOOTHING_FILTER
 
 typedef struct pidCoefficient_s {
@@ -921,7 +937,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
                         pidSetpointDelta = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
                         break;
                     case RC_SMOOTHING_DERIVATIVE_BIQUAD:
-                        pidSetpointDelta = biquadFilterApply(&setpointDerivativeBiquad[axis], pidSetpointDelta);
+                        pidSetpointDelta = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta);
                         break;
                 }
                 if (axis == rcSmoothingDebugAxis) {

src/main/flight/pid.h

@@ -183,3 +183,4 @@ bool crashRecoveryModeActive(void);
 void pidAcroTrainerInit(void);
 void pidSetAcroTrainerState(bool newState);
 void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
+void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);

src/main/interface/cli.c

@@ -3662,20 +3662,24 @@ static void cliRcSmoothing(char *cmdline)
     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 (rcSmoothingAutoCalculate()) {
-        if (avgRxFrameMs == 0) {
+            cliPrint("# Detected RX frame rate: ");
-            cliPrintLine("NO SIGNAL");
+            if (avgRxFrameMs == 0) {
-        } else {
+                cliPrintLine("NO SIGNAL");
-            cliPrintLinef("%d.%dms", avgRxFrameMs / 1000, avgRxFrameMs % 1000);
+            } else {
+                cliPrintLinef("%d.%dms", avgRxFrameMs / 1000, avgRxFrameMs % 1000);
+            }
         }
-        cliPrintLinef("# Auto input cutoff: %dhz", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_AUTO));
+        cliPrint("# Input filter type: ");
+        cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rxConfig()->rc_smoothing_input_type]);
         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));
+        cliPrint("# Derivative filter type: ");
+        cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rxConfig()->rc_smoothing_derivative_type]);
         cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE));
         if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) {
             cliPrintLine("off)");