FF update including second order fitlering of boost, tidying up etc

Include default to no averaging

src/main/blackbox/blackbox.c

@@ -1390,7 +1390,6 @@ static bool blackboxWriteSysinfo(void)
                                                                             currentPidProfile->pid[PID_YAW].F);
 #ifdef USE_INTERPOLATED_SP
         BLACKBOX_PRINT_HEADER_LINE("ff_interpolate_sp", "%d",               currentPidProfile->ff_interpolate_sp);
-        BLACKBOX_PRINT_HEADER_LINE("ff_spike_limit", "%d",                  currentPidProfile->ff_spike_limit);
         BLACKBOX_PRINT_HEADER_LINE("ff_max_rate_limit", "%d",               currentPidProfile->ff_max_rate_limit);
 #endif
         BLACKBOX_PRINT_HEADER_LINE("ff_boost", "%d",                        currentPidProfile->ff_boost);

src/main/cli/settings.c

@@ -1145,7 +1145,6 @@ const clivalue_t valueTable[] = {
 #endif
 #ifdef USE_INTERPOLATED_SP
     { "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

src/main/flight/interpolated_setpoint.c

@@ -35,7 +35,6 @@
 
 static float setpointDeltaImpl[XYZ_AXIS_COUNT];
 static float setpointDelta[XYZ_AXIS_COUNT];
-static uint8_t holdCount[XYZ_AXIS_COUNT];
 
 typedef struct laggedMovingAverageCombined_s {
      laggedMovingAverage_t filter;
@@ -47,11 +46,14 @@ laggedMovingAverageCombined_t  setpointDeltaAvg[XYZ_AXIS_COUNT];
 static float prevSetpointSpeed[XYZ_AXIS_COUNT];
 static float prevAcceleration[XYZ_AXIS_COUNT];
 static float prevRawSetpoint[XYZ_AXIS_COUNT];
+//for smoothing
 static float prevDeltaImpl[XYZ_AXIS_COUNT];
+static float prevBoostAmount[XYZ_AXIS_COUNT];
+
+static uint8_t ffStatus[XYZ_AXIS_COUNT];
 static bool bigStep[XYZ_AXIS_COUNT];
 static uint8_t averagingCount;
 
-// Configuration
 static float ffMaxRateLimit[XYZ_AXIS_COUNT];
 static float ffMaxRate[XYZ_AXIS_COUNT];
 
@@ -69,101 +71,85 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
 
     if (newRcFrame) {
         float rawSetpoint = getRawSetpoint(axis);
-
+        float absRawSetpoint = fabsf(rawSetpoint);
         const float rxInterval = getCurrentRxRefreshRate() * 1e-6f;
         const float rxRate = 1.0f / rxInterval;
         float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
-        float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
+        float absSetpointSpeed = fabsf(setpointSpeed);
-        float setpointSpeedModified = setpointSpeed;
+        float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
-        float setpointAccelerationModified = setpointAcceleration;
+        float setpointAccelerationModifier = 1.0f;
 
-        // Glitch reduction code for identical packets
+        if (setpointSpeed == 0 && absRawSetpoint < 0.98f * ffMaxRate[axis]) {
-        if (fabsf(setpointAcceleration) > 3.0f * fabsf(prevAcceleration[axis])) {
+            // no movement, or sticks at max; ffStatus set
+            // the max stick check is needed to prevent interpolation when arriving at max sticks
+            if (prevSetpointSpeed[axis] == 0) {
+                // no movement on two packets in a row
+                // do nothing now, but may use status = 3 to smooth following packet
+                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;
+                }
+            }
+        } else {
+            // we have movement; let's consider what happened on previous packets, using ffStatus
+            if (ffStatus[axis] == 1) {
+                // was interpolated forward after previous dropped packet after small step
+                // this step is likely twice as tall as it should be
+                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
+        }
+
+        float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
+
+        // determine if this step was a relatively large one, to use when evaluating next packet
+
+        if (absSetpointSpeed > 1.5f * absPrevSetpointSpeed || absPrevSetpointSpeed > 1.5f * absSetpointSpeed){
             bigStep[axis] = true;
         } else {
             bigStep[axis] = false;
         }
 
-        if (setpointSpeed == 0 && fabsf(rawSetpoint) < 0.98f * ffMaxRate[axis]) {
-            // identical packet detected, not at full deflection.
-            // first packet on leaving full deflection always gets full FF
-            if (holdCount[axis] == 0) {
-                // previous packet had movement
-                if (bigStep[axis]) {
-                    // type 1 = interpolate forward where acceleration change is large
-                    setpointSpeedModified = prevSetpointSpeed[axis];
-                    setpointAccelerationModified = prevAcceleration[axis];
-                    holdCount[axis] = 1;
-                } else {
-                    // type 2 = small change, no interpolation needed
-                    setpointSpeedModified = 0.0f;
-                    setpointSpeed = setpointSpeed / 2.0f;
-                    holdCount[axis] = 2;
-                }
-            } else {
-                // it is an unchanged packet after previous unchanged packet
-                // speed and acceleration will be zero, no need to change anything
-                holdCount[axis] = 3;
-            }
-        } else {
-            // we're moving, or sticks are at max
-            if (holdCount[axis] != 0) {
-                // previous step was a duplicate, handle each type differently
-                if (holdCount[axis] == 1) {
-                    // interpolation was applied
-                    // raw setpoint speed of next 'good' packet is twice what it should be
-                    setpointSpeedModified = setpointSpeed / 2.0f;
-                    setpointSpeed = setpointSpeedModified;
-                    // empirically this works best
-                    setpointAccelerationModified = (prevAcceleration[axis] + setpointAcceleration) / 2.0f;
-                } else if (holdCount[axis] == 2) {
-                    // interpolation was not applied
-                } else if (holdCount[axis] == 3) {
-                    // after persistent flat period, no boost
-                    // reduces jitter from boost when flying smooth lines
-                    // but only when no ff_averaging is active, eg hard core race setups
-                    // WARNING: this means no boost if ADC is active on FrSky radios
-                    if (averagingCount > 1) {
-                        setpointAccelerationModified /= averagingCount;
-                    }
-                }
-                holdCount[axis] = 0;
-            }
-        }
-
         // 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 = fabsf(rawSetpoint) / averagingCount;
+            const float rawSetpointCentred = absRawSetpoint / averagingCount;
             if (rawSetpointCentred < 1.0f) {
-                setpointSpeedModified *= rawSetpointCentred;
+                setpointSpeed *= rawSetpointCentred;
-                setpointAccelerationModified *= rawSetpointCentred;
+                setpointAcceleration *= rawSetpointCentred;
-                holdCount[axis] = 4;
             }
         }
 
-        setpointDeltaImpl[axis] = setpointSpeedModified * pidGetDT();
         prevAcceleration[axis] = setpointAcceleration;
-
+     
+        // all values afterwards are small numbers
         setpointAcceleration *= pidGetDT();
-        setpointAccelerationModified *= pidGetDT();
+        setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
+
 
         const float ffBoostFactor = pidGetFfBoostFactor();
-        float clip = 1.0f;
         float boostAmount = 0.0f;
         if (ffBoostFactor != 0.0f) {
-            //calculate clip factor to reduce boost on big spikes
-            if (pidGetSpikeLimitInverse()) {
-                clip = 1 / (1 + (setpointAcceleration * setpointAcceleration * pidGetSpikeLimitInverse()));
-                clip *= clip;
-            }
-            // don't clip first step inwards from max deflection
-            if (fabsf(prevRawSetpoint[axis]) > 0.95f * ffMaxRate[axis] && fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) {
-                clip = 1.0f;
-            }
             // calculate boost and prevent kick-back spike at max deflection
-            if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) {
+            if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
-                boostAmount = ffBoostFactor * setpointAccelerationModified;
+                boostAmount = ffBoostFactor * setpointAcceleration * setpointAccelerationModifier;
             }
         }
 
@@ -172,18 +158,23 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
 
         if (axis == FD_ROLL) {
             DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100));
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(setpointAccelerationModified * 100));
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(setpointAcceleration * 100));
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(setpointAcceleration * 100));
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(((setpointDeltaImpl[axis] + boostAmount) * 100)));
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, holdCount[axis]);
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, ffStatus[axis]);
         }
 
-        setpointDeltaImpl[axis] += boostAmount * clip;
+        // first order smoothing of boost to reduce jitter
-
-        // first order (kind of) smoothing of FF
         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];
         } else {

src/main/flight/pid.c

@@ -121,7 +121,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 
 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 15);
+PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 1);
 
 void resetPidProfile(pidProfile_t *pidProfile)
 {
@@ -202,8 +202,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .idle_p = 50,
         .idle_pid_limit = 200,
         .idle_max_increase = 150,
-        .ff_interpolate_sp = FF_INTERPOLATE_AVG2,
+        .ff_interpolate_sp = FF_INTERPOLATE_ON,
-        .ff_spike_limit = 60,
         .ff_max_rate_limit = 100,
         .ff_smooth_factor = 37,
         .ff_boost = 15,
@@ -246,12 +245,6 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState)
 
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
-float pidGetSpikeLimitInverse()
-{
-    return pidRuntime.ffSpikeLimitInverse;
-}
-
-
 float pidGetFfBoostFactor()
 {
     return pidRuntime.ffBoostFactor;

src/main/flight/pid.h

@@ -196,7 +196,6 @@ 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
     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
@@ -257,7 +256,6 @@ typedef struct pidRuntime_s {
     float antiGravityOsdCutoff;
     float antiGravityThrottleHpf;
     float ffBoostFactor;
-    float ffSpikeLimitInverse;
     float itermAccelerator;
     uint16_t itermAcceleratorGain;
     float feedForwardTransition;
@@ -413,5 +411,4 @@ float pidGetDT();
 float pidGetPidFrequency();
 float pidGetFfBoostFactor();
 float pidGetFfSmoothFactor();
-float pidGetSpikeLimitInverse();
 float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);

src/main/flight/pid_init.c

@@ -204,7 +204,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT));
 
     pidRuntime.ffBoostFactor = (float)pidProfile->ff_boost / 10.0f;
-    pidRuntime.ffSpikeLimitInverse = pidProfile->ff_spike_limit ? 1.0f / ((float)pidProfile->ff_spike_limit / 10.0f) : 0.0f;
 }
 
 void pidInit(const pidProfile_t *pidProfile)