Experimental filter-based rc channel smoothing

Adds an additional rc channel smoothing algorithm that can be used in place of the default rc interpolation. Utilizing a filter-based approach the smoothing has lower latency and is immune to loop time jitter that can introduce artifacts.  Additionally a smoothing filter is added to the setpoint derivative used to produce D-term setpoint weight resulting in a smoother effect on D.

The default setting is to use the previous interpolation logic and there are no changes unless the optional method is selected.

Configuration:

rc_smoothing_type: (INTERPOLATION | FILTER) - defaults to INTERPOLATION

rc_smoothing_input_hz: (0-255) - sets the rc channel input filter cutoff in Hz. Default value of 0 will enable auto calculation based on received RX frame rate.

rc_smoothing_derivative_hz: (0-255) - sets the setpoint weight derivative filter cutoff in Hz. Default value of 0 will enable auto calculation based on received RX frame rate.

rc_smoothing_debug_axis: (ROLL | PITCH | YAW | THROTTLE) - determines which axis is logged in the debug fields

Debug logging:

set debug_mode = RC_SMOOTHING

debug(0) = raw un-smoothed rc channel data
debug(1) = raw un-smoothed setpoint derivative
debug(2) = filtered setpoint derivative before applied to setpoint weight
debug(3) = auto-calculated filter cutoff frequency base after sampling the rx frame rate

Notes:

Currently only enabled for F4/F7 due to flash size limitations

Uses the rc_inter_ch parameter to determine which channels are smoothed (same as default interpolation logic)

The auto filter cutoff calculation will set a cutoff frequency of 30Hz for typical SBUS frames (9ms).  11ms Spektrum will calculate to approximately 25Hz. The user can manually enter the filter cutoffs to be used instead of the auto calculation.  The current default calculation was chosen as a good starting point but may be adjusted in the future.

Setting a lower cutoff frequency will result in more smoothing, but also more delay.

There currently isn't any support for receivers that change their rx frame rate dynamically.  So for CRSF users wishing to use this alternate smoothing method should change their settings to lock the rx frame rate for now. Support for auto-adjusting to new frame rates in flight will likely be added.

src/main/build/debug.c

@@ -71,4 +71,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "RTH",
     "ITERM_RELAX",
     "ACRO_TRAINER",
+    "RC_SMOOTHING",
 };

src/main/build/debug.h

@@ -89,6 +89,7 @@ typedef enum {
     DEBUG_RTH,
     DEBUG_ITERM_RELAX,
     DEBUG_ACRO_TRAINER,
+    DEBUG_RC_SMOOTHING,
     DEBUG_COUNT
 } debugType_e;
 

src/main/fc/fc_rc.c

@@ -58,6 +58,12 @@ static bool reverseMotors = false;
 static applyRatesFn *applyRates;
 uint16_t currentRxRefreshRate;
 
+#ifdef USE_RC_SMOOTHING_FILTER
+#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50
+#define RC_SMOOTHING_FILTER_AUTO_HZ 30.0f // Used to calculate the default cutoff based on rx frame rate. For example, 9ms frame should use 30hz
+#define RC_SMOOTHING_FILTER_AUTO_MS 9.0f  // Formula used: RC_SMOOTHING_FILTER_AUTO_HZ / (measured rx frame delay / RC_SMOOTHING_FILTER_AUTO_HZ)
+#endif // USE_RC_SMOOTHING_FILTER
+
 float getSetpointRate(int axis)
 {
     return setpointRate[axis];
@@ -182,16 +188,12 @@ static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
     }
 }
 
-FAST_CODE FAST_CODE_NOINLINE void processRcCommand(void)
+FAST_CODE FAST_CODE_NOINLINE uint8_t processRcInterpolation(void)
 {
     static float rcCommandInterp[4];
     static float rcStepSize[4];
     static int16_t rcInterpolationStepCount;
 
-    if (isRXDataNew && isAntiGravityModeActive()) {
-        checkForThrottleErrorResetState(currentRxRefreshRate);
-    }
-
     const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT"
     uint16_t rxRefreshRate;
     uint8_t updatedChannel = 0;
@@ -218,12 +220,8 @@ FAST_CODE FAST_CODE_NOINLINE void processRcCommand(void)
                 rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount;
             }
 
-            if (debugMode == DEBUG_RC_INTERPOLATION) {
+           DEBUG_SET(DEBUG_RC_INTERPOLATION, 0, lrintf(rcCommand[0]));
-                debug[0] = lrintf(rcCommand[0]);
+           DEBUG_SET(DEBUG_RC_INTERPOLATION, 1, lrintf(currentRxRefreshRate / 1000));
-                debug[1] = lrintf(currentRxRefreshRate / 1000);
-                //debug[1] = lrintf(rcCommandInterp[0]);
-                //debug[1] = lrintf(rcStepSize[0]*100);
-            }
         } else {
             rcInterpolationStepCount--;
         }
@@ -239,6 +237,105 @@ FAST_CODE FAST_CODE_NOINLINE void processRcCommand(void)
         rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping
     }
 
+    DEBUG_SET(DEBUG_RC_INTERPOLATION, 2, rcInterpolationStepCount);
+
+    return updatedChannel;
+
+}
+
+#ifdef USE_RC_SMOOTHING_FILTER
+FAST_CODE FAST_CODE_NOINLINE uint8_t processRcSmoothingFilter(void)
+{
+
+    uint8_t updatedChannel = 0;
+
+    static float lastRxData[4];
+    static pt1Filter_t rcCommandFilter[4];
+    static bool initialized = false;
+    static bool filterInitialized = false;
+    static float rxFrameTimeSum;
+    static int rxFrameCount;
+    static uint16_t defaultCutoffFrequency;
+    static uint16_t filterCutoffFrequency;
+    static uint16_t derivativeCutoffFrequency;
+    static uint8_t interpolationChannels;
+
+    if (!initialized) {
+        initialized = true;
+        interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT"
+        filterCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff;
+        derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff;
+    }
+
+    if (isRXDataNew) {
+        for (int i = 0; i < interpolationChannels; i++) {
+            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
+        if ((filterCutoffFrequency == 0) || (derivativeCutoffFrequency == 0)) {
+            if (rxIsReceivingSignal()) {
+                rxFrameTimeSum += currentRxRefreshRate;
+                rxFrameCount++;
+                if (rxFrameCount >= RC_SMOOTHING_FILTER_TRAINING_SAMPLES) {
+                    const float avgRxFrameRate = rxFrameTimeSum / rxFrameCount / 1000.0f;
+                    defaultCutoffFrequency = lrintf(RC_SMOOTHING_FILTER_AUTO_HZ / (avgRxFrameRate / RC_SMOOTHING_FILTER_AUTO_MS));
+                    filterCutoffFrequency = (filterCutoffFrequency == 0) ? defaultCutoffFrequency : filterCutoffFrequency;
+                    derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultCutoffFrequency : derivativeCutoffFrequency;                
+                }
+            } else {
+                rxFrameTimeSum = 0;
+                rxFrameCount = 0;
+            }
+        }
+    }
+
+    DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis]));
+    DEBUG_SET(DEBUG_RC_SMOOTHING, 3, defaultCutoffFrequency);
+
+    // Once we've determined the filter cutoff frequencies then initialize the filters
+    if (!filterInitialized && (targetPidLooptime > 0) && (filterCutoffFrequency != 0) && (derivativeCutoffFrequency != 0)) {
+        const float dT = targetPidLooptime * 0.000001f;
+        for (int i = 0; i < interpolationChannels; i++) {
+            pt1FilterInit(&rcCommandFilter[i], pt1FilterGain(filterCutoffFrequency, dT));
+        }
+        pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis);
+        filterInitialized = true;
+    }
+
+    for (updatedChannel = ROLL; updatedChannel < interpolationChannels; updatedChannel++) {
+        if (filterInitialized) {
+                rcCommand[updatedChannel] = pt1FilterApply(&rcCommandFilter[updatedChannel], lastRxData[updatedChannel]);
+        } else {
+            // If filter isn't initialized yet then use the actual unsmoothed rx channel data
+            rcCommand[updatedChannel] = lastRxData[updatedChannel];
+        }
+    }
+
+    return interpolationChannels;
+}
+#endif // USE_RC_SMOOTHING_FILTER
+
+FAST_CODE FAST_CODE_NOINLINE void processRcCommand(void)
+{
+    uint8_t updatedChannel;
+
+    if (isRXDataNew && isAntiGravityModeActive()) {
+        checkForThrottleErrorResetState(currentRxRefreshRate);
+    }
+
+    switch (rxConfig()->rc_smoothing_type) {
+#ifdef USE_RC_SMOOTHING_FILTER
+    case RC_SMOOTHING_TYPE_FILTER:
+        updatedChannel = processRcSmoothingFilter();
+        break;
+#endif // USE_RC_SMOOTHING_FILTER
+    case RC_SMOOTHING_TYPE_INTERPOLATION:
+    default:
+        updatedChannel = processRcInterpolation();
+        break;
+    }
+
     if (isRXDataNew || updatedChannel) {
         const uint8_t maxUpdatedAxis = isRXDataNew ? FD_YAW : MIN(updatedChannel, FD_YAW); // throttle channel doesn't require rate calculation
 #if defined(SIMULATOR_BUILD)
@@ -252,10 +349,7 @@ FAST_CODE FAST_CODE_NOINLINE void processRcCommand(void)
             calculateSetpointRate(axis);
         }
 
-        if (debugMode == DEBUG_RC_INTERPOLATION) {
+        DEBUG_SET(DEBUG_RC_INTERPOLATION, 3, setpointRate[0]);
-            debug[2] = rcInterpolationStepCount;
-            debug[3] = setpointRate[0];
-        }
 
         // Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
         if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {

src/main/fc/rc_controls.h

@@ -58,6 +58,12 @@ typedef enum {
     RC_SMOOTHING_MANUAL
 } rcSmoothing_t;
 
+typedef enum {
+    RC_SMOOTHING_TYPE_INTERPOLATION,
+    RC_SMOOTHING_TYPE_FILTER
+} rcInterpolationType_e;
+
+
 #define ROL_LO (1 << (2 * ROLL))
 #define ROL_CE (3 << (2 * ROLL))
 #define ROL_HI (2 << (2 * ROLL))

src/main/flight/pid.c

@@ -210,6 +210,12 @@ static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow;
 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 bool setpointDerivativeLpfInitialized;
+static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
+#endif // USE_RC_SMOOTHING_FILTER
+
 void pidInitFilters(const pidProfile_t *pidProfile)
 {
     BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
@@ -295,6 +301,16 @@ void pidInitFilters(const pidProfile_t *pidProfile)
 #endif
 }
 
+#ifdef USE_RC_SMOOTHING_FILTER
+void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis) {
+    setpointDerivativeLpfInitialized = true;
+    rcSmoothingDebugAxis = debugAxis;
+    for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
+        pt1FilterInit(&setpointDerivativeLpf[axis], pt1FilterGain(filterCutoff, dT));
+    }
+}
+#endif // USE_RC_SMOOTHING_FILTER
+
 typedef struct pidCoefficient_s {
     float Kp;
     float Ki;
@@ -862,17 +878,30 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
 
             pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor;
 
+            float pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
+
+#ifdef USE_RC_SMOOTHING_FILTER
+            if (axis == rcSmoothingDebugAxis) {
+                DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
+            }
+            if ((dynCd != 0) && setpointDerivativeLpfInitialized) {
+                pidSetpointDelta = pt1FilterApply(&setpointDerivativeLpf[axis], pidSetpointDelta);
+
+                if (axis == rcSmoothingDebugAxis) {
+                    DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(pidSetpointDelta * 100.0f));
+                }
+            }
+#endif // USE_RC_SMOOTHING_FILTER
+
             const float pidFeedForward =
-                pidCoefficient[axis].Kd * dynCd * transition *
+                pidCoefficient[axis].Kd * dynCd * transition * pidSetpointDelta * tpaFactor / dT;
-                (currentPidSetpoint - previousPidSetpoint[axis]) * tpaFactor / dT;
 #if defined(USE_SMART_FEEDFORWARD)
             bool addFeedforward = true;
             if (smartFeedforward) {
                 if (pidData[axis].P * pidFeedForward > 0) {
                     if (ABS(pidFeedForward) > ABS(pidData[axis].P)) {
                         pidData[axis].P = 0;
-                    }
+                    } else {
-                    else {
                         addFeedforward = false;
                     }
                 }

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

src/main/interface/settings.c

@@ -349,6 +349,15 @@ static const char * const lookupTableAcroTrainerDebug[] = {
 };
 #endif // USE_ACRO_TRAINER
 
+#ifdef USE_RC_SMOOTHING_FILTER
+static const char * const lookupTableRcSmoothingType[] = {
+    "INTERPOLATION", "FILTER"
+};
+static const char * const lookupTableRcSmoothingDebug[] = {
+    "ROLL", "PITCH", "YAW", "THROTTLE"
+};
+#endif // USE_RC_SMOOTHING_FILTER
+
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
 
 const lookupTableEntry_t lookupTables[] = {
@@ -430,6 +439,10 @@ const lookupTableEntry_t lookupTables[] = {
 #ifdef USE_ACRO_TRAINER
     LOOKUP_TABLE_ENTRY(lookupTableAcroTrainerDebug),
 #endif // USE_ACRO_TRAINER
+#ifdef USE_RC_SMOOTHING_FILTER
+    LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingType),
+    LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDebug),
+#endif // USE_RC_SMOOTHING_FILTER
 };
 
 #undef LOOKUP_TABLE_ENTRY
@@ -525,6 +538,14 @@ const clivalue_t valueTable[] = {
     { "rc_interp",                  VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_INTERPOLATION }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolation) },
     { "rc_interp_ch",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_INTERPOLATION_CHANNELS }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolationChannels) },
     { "rc_interp_int",              VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 50 }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolationInterval) },
+
+#ifdef USE_RC_SMOOTHING_FILTER
+    { "rc_smoothing_type",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_type) },
+    { "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) },
+#endif // USE_RC_SMOOTHING_FILTER
+
     { "fpv_mix_degrees",            VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_RX_CONFIG, offsetof(rxConfig_t, fpvCamAngleDegrees) },
     { "max_aux_channels",           VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, MAX_AUX_CHANNEL_COUNT }, PG_RX_CONFIG, offsetof(rxConfig_t, max_aux_channel) },
 #ifdef USE_SERIAL_RX

src/main/interface/settings.h

@@ -104,6 +104,10 @@ typedef enum {
 #ifdef USE_ACRO_TRAINER
     TABLE_ACRO_TRAINER_DEBUG,
 #endif // USE_ACRO_TRAINER
+#ifdef USE_RC_SMOOTHING_FILTER
+    TABLE_RC_SMOOTHING_TYPE,
+    TABLE_RC_SMOOTHING_DEBUG,
+#endif // USE_RC_SMOOTHING_FILTER
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;
 

src/main/pg/rx.c

@@ -59,7 +59,11 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
         .rcInterpolationInterval = 19,
         .fpvCamAngleDegrees = 0,
         .airModeActivateThreshold = 32,
-        .max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT
+        .max_aux_channel = DEFAULT_AUX_CHANNEL_COUNT,
+        .rc_smoothing_type = RC_SMOOTHING_TYPE_INTERPOLATION,
+        .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
     );
 
 #ifdef RX_CHANNELS_TAER

src/main/pg/rx.h

@@ -50,6 +50,11 @@ typedef struct rxConfig_s {
     uint8_t max_aux_channel;
     uint8_t rssi_src_frame_errors;          // true to use frame drop flags in the rx protocol
     int8_t rssi_offset;                     // offset applied to the RSSI value before it is returned
+
+    uint8_t rc_smoothing_type;              // Determines the smoothing algorithm to use: INTERPOLATION or FILTER
+    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
 } rxConfig_t;
 
 PG_DECLARE(rxConfig_t, rxConfig);

src/main/target/common_fc_pre.h

@@ -209,6 +209,7 @@
 #define USE_GPS_UBLOX
 #define USE_GPS_RESCUE
 #define USE_OSD_ADJUSTMENTS
+#define USE_RC_SMOOTHING_FILTER
 #define USE_SENSOR_NAMES
 #define USE_SERIALRX_JETIEXBUS
 #define USE_TELEMETRY_IBUS