feed forward jitter improvements

src/main/cli/settings.c

@@ -1164,6 +1164,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_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) },
+    { "ff_jitter_reduction",        VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_jitter_factor) },
 #endif
     { "ff_boost",                   VAR_UINT8 | PROFILE_VALUE,  .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },
 

src/main/cms/cms_menu_imu.c

@@ -519,6 +519,7 @@ static uint8_t cmsx_iterm_relax_cutoff;
 #ifdef USE_INTERPOLATED_SP
 static uint8_t cmsx_ff_interpolate_sp;
 static uint8_t cmsx_ff_smooth_factor;
+static uint8_t cmsx_ff_jitter_factor;
 #endif
 
 static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
@@ -561,6 +562,7 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
 #ifdef USE_INTERPOLATED_SP
     cmsx_ff_interpolate_sp = pidProfile->ff_interpolate_sp;
     cmsx_ff_smooth_factor = pidProfile->ff_smooth_factor;
+    cmsx_ff_jitter_factor = pidProfile->ff_jitter_factor;
 #endif
 
 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
@@ -608,6 +610,7 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
 #ifdef USE_INTERPOLATED_SP
     pidProfile->ff_interpolate_sp = cmsx_ff_interpolate_sp;
     pidProfile->ff_smooth_factor = cmsx_ff_smooth_factor;
+    pidProfile->ff_jitter_factor = cmsx_ff_jitter_factor;
 #endif
 
 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
@@ -625,6 +628,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
 #ifdef USE_INTERPOLATED_SP
     { "FF MODE",       OME_TAB,    NULL, &(OSD_TAB_t)    { &cmsx_ff_interpolate_sp,  4, lookupTableInterpolatedSetpoint}, 0 },
     { "FF SMOOTHNESS", OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_ff_smooth_factor,     0,     75,   1  }   , 0 },
+    { "FF JITTER",     OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_ff_jitter_factor,     0,     20,   1  }   , 0 },
 #endif
     { "FF BOOST",    OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_ff_boost,               0,     50,   1  }   , 0 },
     { "ANGLE STR",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleStrength,          0,    200,   1  }   , 0 },

src/main/fc/rc.c

@@ -61,7 +61,11 @@ typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCom
 #ifdef USE_INTERPOLATED_SP
 // Setpoint in degrees/sec before RC-Smoothing is applied
 static float rawSetpoint[XYZ_AXIS_COUNT];
+static float oldRcCommand[XYZ_AXIS_COUNT];
+static bool isDuplicate[XYZ_AXIS_COUNT];
+float rcCommandDelta[XYZ_AXIS_COUNT];
 #endif
+
 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
 static float throttlePIDAttenuation;
 static bool reverseMotors = false;
@@ -132,6 +136,11 @@ float getRawSetpoint(int axis)
     return rawSetpoint[axis];
 }
 
+float getRcCommandDelta(int axis)
+{
+    return rcCommandDelta[axis];
+}
+
 #endif
 
 #define THROTTLE_LOOKUP_LENGTH 12
@@ -636,6 +645,9 @@ FAST_CODE void processRcCommand(void)
 #ifdef USE_INTERPOLATED_SP
     if (isRxDataNew) {
         for (int i = FD_ROLL; i <= FD_YAW; i++) {
+            isDuplicate[i] = (oldRcCommand[i] == rcCommand[i]);
+            rcCommandDelta[i] = fabsf(rcCommand[i] - oldRcCommand[i]);
+            oldRcCommand[i] = rcCommand[i];
             float rcCommandf;
             if (i == FD_YAW) {
                 rcCommandf = rcCommand[i] / rcCommandYawDivider;

src/main/fc/rc.h

@@ -51,6 +51,7 @@ rcSmoothingFilter_t *getRcSmoothingData(void);
 bool rcSmoothingAutoCalculate(void);
 bool rcSmoothingInitializationComplete(void);
 float getRawSetpoint(int axis);
+float getRcCommandDelta(int axis);
 float applyCurve(int axis, float deflection);
 bool getShouldUpdateFf();
 void updateRcRefreshRate(timeUs_t currentTimeUs);

src/main/flight/interpolated_setpoint.c

@@ -43,15 +43,12 @@ typedef struct laggedMovingAverageCombined_s {
 
 laggedMovingAverageCombined_t  setpointDeltaAvg[XYZ_AXIS_COUNT];
 
-static float prevSetpointSpeed[XYZ_AXIS_COUNT];
+static float prevSetpoint[XYZ_AXIS_COUNT]; // equals raw unless interpolated 
-static float prevAcceleration[XYZ_AXIS_COUNT];
+static float prevSetpointSpeed[XYZ_AXIS_COUNT]; // equals raw unless interpolated
-static float prevRawSetpoint[XYZ_AXIS_COUNT];
+static float prevAcceleration[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation
-//for smoothing
+static float prevRcCommandDelta[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation
-static float prevDeltaImpl[XYZ_AXIS_COUNT];
-static float prevBoostAmount[XYZ_AXIS_COUNT];
 
-static uint8_t ffStatus[XYZ_AXIS_COUNT];
+static bool prevDuplicatePacket[XYZ_AXIS_COUNT]; // to identify multiple identical packets
-static bool bigStep[XYZ_AXIS_COUNT];
 static uint8_t averagingCount;
 
 static float ffMaxRateLimit[XYZ_AXIS_COUNT];
@@ -70,112 +67,86 @@ void interpolatedSpInit(const pidProfile_t *pidProfile) {
 FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type) {
 
     if (newRcFrame) {
-        float rawSetpoint = getRawSetpoint(axis);
+        float rcCommandDelta = getRcCommandDelta(axis);
-        float absRawSetpoint = fabsf(rawSetpoint);
+        float setpoint = getRawSetpoint(axis);
         const float rxInterval = getCurrentRxRefreshRate() * 1e-6f;
         const float rxRate = 1.0f / rxInterval;
-        float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
+        float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
-        float absSetpointSpeed = fabsf(setpointSpeed);
         float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
-        float setpointAccelerationModifier = 1.0f;
+        float setpointAcceleration = 0.0f;
+        const float ffSmoothFactor = pidGetFfSmoothFactor();
+        const float ffJitterFactor = pidGetFfJitterFactor();
 
-        if (setpointSpeed == 0 && absRawSetpoint < 0.98f * ffMaxRate[axis]) {
+        // calculate an attenuator from average of two most recent rcCommand deltas vs jitter threshold
-            // no movement, or sticks at max; ffStatus set
+        float ffAttenuator = 1.0f;
-            // the max stick check is needed to prevent interpolation when arriving at max sticks
+        if (ffJitterFactor) {
-            if (prevSetpointSpeed[axis] == 0) {
+            if (rcCommandDelta < ffJitterFactor) {
-                // no movement on two packets in a row
+                ffAttenuator = MAX(1.0f - ((rcCommandDelta + prevRcCommandDelta[axis]) / 2.0f) / ffJitterFactor, 0.0f);
-                // do nothing now, but may use status = 3 to smooth following packet
+                ffAttenuator = 1.0f - ffAttenuator * ffAttenuator;
-                ffStatus[axis] = 3;
-            } else {
-                // there was movement on previous packet, now none
-                if (bigStep[axis] == true) {
-                    // previous movement was big; likely an early FrSky packet
-                    // don't project these forward or we get a sustained large spike
-                    ffStatus[axis] = 2;
-                } else {
-                    // likely a dropped packet
-                    // interpolate forward using previous setpoint speed and acceleration
-                    setpointSpeed = prevSetpointSpeed[axis] + prevAcceleration[axis];
-                    // use status = 1 to halve the step for the next packet
-                    ffStatus[axis] = 1;
-                }
             }
+        }
+
+        // interpolate setpoint if necessary
+        if (rcCommandDelta == 0.0f) {
+            if (prevDuplicatePacket[axis] == false && fabsf(setpoint) < 0.98f * ffMaxRate[axis]) {
+                // first duplicate after movement
+                // interpolate rawSetpoint by adding (speed + acceleration) * attenuator to previous setpoint
+                setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis]) * ffAttenuator * rxInterval;
+                // recalculate setpointSpeed and (later) acceleration from this new setpoint value
+                setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
+            }
+            prevDuplicatePacket[axis] = true;
         } else {
-            // we have movement; let's consider what happened on previous packets, using ffStatus
+            // movement!
-            if (ffStatus[axis] != 0) {
+            if (prevDuplicatePacket[axis] == true) {
-                if (ffStatus[axis] == 1) {
+                // don't boost the packet after a duplicate, the FF alone is enough, usually
-                    // was interpolated forward after previous dropped packet after small step
+                // in part because after a duplicate, the raw up-step is large, so the jitter attenuator is less active
-                    // this step is likely twice as tall as it should be
+                ffAttenuator = 0.0f;
-                    setpointSpeed = setpointSpeed / 2.0f;
-                } else if (ffStatus[axis] == 2) {
-                    // we are doing nothing for these to avoid exaggerating the FrSky early packet problem
-                } else if (ffStatus[axis] == 3) {
-                    // movement after nothing on previous two packets
-                    // reduce boost when higher averaging is used to improve slow stick smoothness
-                    setpointAccelerationModifier /= (averagingCount + 1);
-                }
-                ffStatus[axis] = 0;
-                // all is normal
             }
+            prevDuplicatePacket[axis] = false;
+        }
+        prevSetpoint[axis] = setpoint;
+
+        if (axis == FD_ROLL) {
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(setpoint)); // setpoint after interpolations
         }
 
-        float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
+        float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
 
-        // determine if this step was a relatively large one, to use when evaluating next packet
+        // calculate acceleration, smooth and attenuate it
+        setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
+        setpointAcceleration = prevAcceleration[axis] + ffSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
+        setpointAcceleration *= ffAttenuator;
 
-        if (absSetpointSpeed > 1.5f * absPrevSetpointSpeed || absPrevSetpointSpeed > 1.5f * absSetpointSpeed){
+        // smooth setpointSpeed but don't attenuate
-            bigStep[axis] = true;
+        setpointSpeed = prevSetpointSpeed[axis] + ffSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
-        } else {
-            bigStep[axis] = false;
-        }
-
-        // smooth deadband type suppression of FF jitter when sticks are at or returning to centre
-        // only when ff_averaging is 3 or more, for HD or cinematic flying
-        if (averagingCount > 2) {
-            const float rawSetpointCentred = absRawSetpoint / averagingCount;
-            if (rawSetpointCentred < 1.0f) {
-                setpointSpeed *= rawSetpointCentred;
-                setpointAcceleration *= rawSetpointCentred;
-            }
-        }
 
+        prevSetpointSpeed[axis] = setpointSpeed;
         prevAcceleration[axis] = setpointAcceleration;
+        prevRcCommandDelta[axis] = rcCommandDelta;
 
-        // all values afterwards are small numbers
         setpointAcceleration *= pidGetDT();
         setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
 
-
+        // calculate boost and prevent kick-back spike at max deflection
         const float ffBoostFactor = pidGetFfBoostFactor();
         float boostAmount = 0.0f;
-        if (ffBoostFactor != 0.0f) {
+        if (ffBoostFactor) {
-            // calculate boost and prevent kick-back spike at max deflection
+            if (fabsf(setpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
-            if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
+                boostAmount = ffBoostFactor * setpointAcceleration;
-                boostAmount = ffBoostFactor * setpointAcceleration * setpointAccelerationModifier;
             }
         }
 
-        prevSetpointSpeed[axis] = setpointSpeed;
-        prevRawSetpoint[axis] = rawSetpoint;
-
         if (axis == FD_ROLL) {
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100));
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100.0f)); // base FF
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(setpointAcceleration * 100));
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(boostAmount * 100.0f)); // boost amount
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(((setpointDeltaImpl[axis] + boostAmount) * 100)));
+            // debug 2 is interpolated setpoint, above
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, ffStatus[axis]);
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference
         }
 
-        // first order smoothing of boost to reduce jitter
+        // add boost to base feed forward
-        const float ffSmoothFactor = pidGetFfSmoothFactor();
-        boostAmount = prevBoostAmount[axis] + ffSmoothFactor * (boostAmount - prevBoostAmount[axis]);
-        prevBoostAmount[axis] = boostAmount;
-
         setpointDeltaImpl[axis] += boostAmount;
 
-        // first order smoothing of FF (second order boost filtering since boost filtered twice)
-        setpointDeltaImpl[axis] = prevDeltaImpl[axis] + ffSmoothFactor * (setpointDeltaImpl[axis] - prevDeltaImpl[axis]);
-        prevDeltaImpl[axis] = setpointDeltaImpl[axis];
-
         // apply averaging
         if (type == FF_INTERPOLATE_ON) {
             setpointDelta[axis] = setpointDeltaImpl[axis];

src/main/flight/pid.c

@@ -205,7 +205,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .dyn_idle_max_increase = 150,
         .ff_interpolate_sp = FF_INTERPOLATE_ON,
         .ff_max_rate_limit = 100,
-        .ff_smooth_factor = 0,
+        .ff_smooth_factor = 37,
+        .ff_jitter_factor = 7,
         .ff_boost = 15,
         .dyn_lpf_curve_expo = 5,
         .level_race_mode = false,
@@ -266,6 +267,11 @@ float pidGetFfSmoothFactor()
 {
     return pidRuntime.ffSmoothFactor;
 }
+
+float pidGetFfJitterFactor()
+{
+    return pidRuntime.ffJitterFactor;
+}
 #endif
 
 void pidResetIterm(void)

src/main/flight/pid.h

@@ -210,6 +210,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_smooth_factor;               // Amount of smoothing for interpolated FF steps
+    uint8_t ff_jitter_factor;               // Number of RC steps below which to attenuate FF
     uint8_t dyn_lpf_curve_expo;             // set the curve for dynamic dterm lowpass filter
     uint8_t level_race_mode;                // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode
     uint8_t vbat_sag_compensation;          // Reduce motor output by this percentage of the maximum compensation amount
@@ -385,6 +386,7 @@ typedef struct pidRuntime_s {
 #ifdef USE_INTERPOLATED_SP
     ffInterpolationType_t ffFromInterpolatedSetpoint;
     float ffSmoothFactor;
+    float ffJitterFactor;
 #endif
 } pidRuntime_t;
 
@@ -439,4 +441,5 @@ float pidGetDT();
 float pidGetPidFrequency();
 float pidGetFfBoostFactor();
 float pidGetFfSmoothFactor();
+float pidGetFfJitterFactor();
 float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);

src/main/flight/pid_init.c

@@ -392,6 +392,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
         // set automatically according to boost amount, limit to 0.5 for auto
         pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->ff_boost) * 2.0f / 100.0f);
     }
+    pidRuntime.ffJitterFactor = pidProfile->ff_jitter_factor;
     interpolatedSpInit(pidProfile);
 #endif