Interpolated feed forward smoothing

src/main/cli/settings.c

@@ -476,7 +476,7 @@ static const char * const lookupTableOffOnAuto[] = {
 };
 
 static const char* const lookupTableInterpolatedSetpoint[] = {
-    "OFF", "ON", "AVERAGED"
+    "OFF", "ON", "AVERAGED_2", "AVERAGED_3", "AVERAGED_4"
 };
 
 static const char* const lookupTableDshotBitbangedTimer[] = {
@@ -1097,6 +1097,7 @@ const clivalue_t valueTable[] = {
     { "ff_interpolate_sp",          VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_INTERPOLATED_SP}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) },
     { "ff_spike_limit",             VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 255}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_spike_limit) },
     { "ff_max_rate_limit",          VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) },
+    { "ff_smooth_factor",           VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_smooth_factor) },
 #endif
     { "ff_boost",                   VAR_UINT8 | PROFILE_VALUE,  .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },
 

src/main/flight/interpolated_setpoint.c

@@ -28,15 +28,24 @@
 #include "fc/rc.h"
 #include "flight/interpolated_setpoint.h"
 
+#define PREV_BIG_STEP 1000.0f //threshold for size of jump of packet before the identical data packet
+
 static float setpointDeltaImpl[XYZ_AXIS_COUNT];
-static float prevSetpointDeltaImpl[XYZ_AXIS_COUNT];
 static float setpointDelta[XYZ_AXIS_COUNT];
+static uint8_t holdCount[XYZ_AXIS_COUNT];
 
+typedef struct laggedMovingAverageCombined_u {
+     laggedMovingAverage_t filter;
+     float buf[4];
+} laggedMovingAverageCombined_t;
 
+laggedMovingAverageCombined_t  setpointDeltaAvg[XYZ_AXIS_COUNT];
+ 
 static float prevSetpointSpeed[XYZ_AXIS_COUNT];
+static float prevAcceleration[XYZ_AXIS_COUNT];
 static float prevRawSetpoint[XYZ_AXIS_COUNT];
-static float prevSetpointAcceleration[XYZ_AXIS_COUNT];
+static float prevDeltaImpl[XYZ_AXIS_COUNT];
-
+static bool bigStep[XYZ_AXIS_COUNT];
 
 // Configuration
 static float ffMaxRateLimit[XYZ_AXIS_COUNT];
@@ -44,9 +53,11 @@ static float ffMaxRate[XYZ_AXIS_COUNT];
 
 void interpolatedSpInit(const pidProfile_t *pidProfile) {
     const float ffMaxRateScale = pidProfile->ff_max_rate_limit * 0.01f;
+    uint8_t j = pidProfile->ff_interpolate_sp;
     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
         ffMaxRate[i] = applyCurve(i, 1.0f);
         ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale;
+        laggedMovingAverageInit(&setpointDeltaAvg[i].filter, j, (float *)&setpointDeltaAvg[i].buf[0]);
     }
 }
 
@@ -58,9 +69,46 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
         const float rxInterval = currentRxRefreshRate * 1e-6f;
         const float rxRate = 1.0f / rxInterval;
 
-        const float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
+        float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
-        const float setpointAcceleration = (setpointSpeed - prevSetpointSpeed[axis]) * pidGetDT();
+        float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
+        const uint8_t holdSteps = 2;
 
+        // Glitch reduction code for identical packets
+        if (setpointSpeed == 0 && fabsf(rawSetpoint) < 0.98f * ffMaxRate[axis]) {
+            // identical packets, not at full deflection
+            if (holdCount[axis] < holdSteps && fabsf(rawSetpoint) > 2.0f && !bigStep[axis]) {
+                // holding the entire previous speed is best for missed packets, but bad for early packets
+                setpointSpeed = prevSetpointSpeed[axis] + prevAcceleration[axis];
+                setpointAcceleration = prevAcceleration[axis];
+                holdCount[axis] += 1;
+            } else {
+                // identical packets for more than hold steps, or prev big step
+                // lock acceleration to zero and don't interpolate forward until sticks move again
+                holdCount[axis] = holdSteps + 1;
+                setpointAcceleration = 0.0f;
+            }
+        } else {
+            // we're moving, or sticks are at max
+            if (holdCount[axis] == 2) {
+                // we are after an identical packet, and the one before was a normal step up,
+                // so raw step speed and acceleration of next 'good' packet is twice what it should be
+                setpointSpeed /= 2.0f;
+                setpointAcceleration /= 2.0f;
+            }
+            if (holdCount[axis] == 3) {
+                // we are starting to move after a gap after a big step up, or persistent flat period, so accelerate gently
+                setpointAcceleration = 0.0f;
+            }
+            holdCount[axis] = 1;
+            if (fabsf(setpointAcceleration - prevAcceleration[axis]) > PREV_BIG_STEP) {
+                bigStep[axis] = true;
+            } else {
+                bigStep[axis] = false;
+            }
+        }
+
+        prevAcceleration[axis] = setpointAcceleration;
+        setpointAcceleration *= pidGetDT();
         setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
 
         const float ffBoostFactor = pidGetFfBoostFactor();
@@ -75,25 +123,34 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
             if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) {
                 boostAmount = ffBoostFactor * setpointAcceleration;
             }
+            // no clip for first step inwards from max deflection
+            if (fabsf(prevRawSetpoint[axis]) > 0.95f * ffMaxRate[axis] && fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) {
+                clip = 1.0f;
+            }
         }
+
         prevSetpointSpeed[axis] = setpointSpeed;
-        prevSetpointAcceleration[axis] = setpointAcceleration;
         prevRawSetpoint[axis] = rawSetpoint;
+
         if (axis == FD_ROLL) {
             DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, setpointDeltaImpl[axis] * 1000);
             DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, boostAmount * 1000);
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, boostAmount * clip * 1000);
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, bigStep[axis]);
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, clip * 100);
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, holdCount[axis]);
         }
+
         setpointDeltaImpl[axis] += boostAmount * clip;
+        // first order filter FF
+        const float ffSmoothFactor = pidGetFfSmoothFactor();
+        setpointDeltaImpl[axis] = prevDeltaImpl[axis] + ffSmoothFactor * (setpointDeltaImpl[axis] - prevDeltaImpl[axis]);
+        prevDeltaImpl[axis] = setpointDeltaImpl[axis];
+
         if (type == FF_INTERPOLATE_ON) {
             setpointDelta[axis] = setpointDeltaImpl[axis];
         } else {
-            setpointDelta[axis] = 0.5f * (setpointDeltaImpl[axis] + prevSetpointDeltaImpl[axis]);
+            setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDeltaImpl[axis]);
-            prevSetpointDeltaImpl[axis] = setpointDeltaImpl[axis];
         }
     }
-    
     return setpointDelta[axis];
 }
 

src/main/flight/interpolated_setpoint.h

@@ -28,7 +28,9 @@
 typedef enum ffInterpolationType_e {
     FF_INTERPOLATE_OFF,
     FF_INTERPOLATE_ON,
-    FF_INTERPOLATE_AVG
+    FF_INTERPOLATE_AVG2,
+    FF_INTERPOLATE_AVG3,
+    FF_INTERPOLATE_AVG4
 } ffInterpolationType_t;
 
 void interpolatedSpInit(const pidProfile_t *pidProfile);

src/main/flight/pid.c

@@ -212,9 +212,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .idle_p = 50,
         .idle_pid_limit = 200,
         .idle_max_increase = 150,
-        .ff_interpolate_sp = FF_INTERPOLATE_AVG,
+        .ff_interpolate_sp = FF_INTERPOLATE_AVG2,
-        .ff_spike_limit = 60,
+        .ff_spike_limit = 50,
         .ff_max_rate_limit = 100,
+        .ff_smooth_factor = 37,
         .ff_boost = 15,
     );
 #ifndef USE_D_MIN
@@ -314,6 +315,7 @@ static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2;
 static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
 
 static FAST_RAM_ZERO_INIT float ffBoostFactor;
+static FAST_RAM_ZERO_INIT float ffSmoothFactor;
 static FAST_RAM_ZERO_INIT float ffSpikeLimitInverse;
 
 float pidGetSpikeLimitInverse()
@@ -327,6 +329,11 @@ float pidGetFfBoostFactor()
     return ffBoostFactor;
 }
 
+float pidGetFfSmoothFactor()
+{
+    return ffSmoothFactor;
+}
+
 
 void pidInitFilters(const pidProfile_t *pidProfile)
 {
@@ -738,6 +745,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
 #endif
 #ifdef USE_INTERPOLATED_SP
     ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
+    ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
     interpolatedSpInit(pidProfile);
 #endif
 }

src/main/flight/pid.h

@@ -184,6 +184,7 @@ typedef struct pidProfile_s {
     uint8_t ff_interpolate_sp;              // Calculate FF from interpolated setpoint
     uint8_t ff_max_rate_limit;              // Maximum setpoint rate percentage for FF
     uint8_t ff_spike_limit;                 // FF stick extrapolation lookahead period in ms
+    uint8_t ff_smooth_factor;               // Amount of smoothing for interpolated FF steps
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -261,4 +262,5 @@ float pidGetPreviousSetpoint(int axis);
 float pidGetDT();
 float pidGetPidFrequency();
 float pidGetFfBoostFactor();
+float pidGetFfSmoothFactor();
 float pidGetSpikeLimitInverse();