various ff2_0 improvements

simplify

double period averaging

fix calc

ff changes

in between

fixes

clip * clip and cleanup

address review feedback

more review feedback

10 to 100

src/main/cli/settings.c

@@ -473,6 +473,11 @@ static const char * const lookupTableDshotBitbang[] = {
     "OFF", "ON", "AUTO"
 };
 
+static const char* const lookupTableInterpolatedSetpoint[] = {
+    "OFF", "ON", "AVERAGED"
+};
+
+
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
 
 const lookupTableEntry_t lookupTables[] = {
@@ -588,6 +593,7 @@ const lookupTableEntry_t lookupTables[] = {
 
     LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource),
     LOOKUP_TABLE_ENTRY(lookupTableDshotBitbang),
+    LOOKUP_TABLE_ENTRY(lookupTableInterpolatedSetpoint),
 };
 
 #undef LOOKUP_TABLE_ENTRY
@@ -1073,10 +1079,9 @@ const clivalue_t valueTable[] = {
     { "transient_throttle_limit",   VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) },
 #endif
 #ifdef USE_INTERPOLATED_SP
-    { "ff_interpolate_sp",          VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_OFF_ON}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_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_spread",                  VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 50}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_spread) },
+    { "ff_jerk_limit",              VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 255}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_jerk_limit) },
     { "ff_max_rate_limit",          VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) },
-    { "ff_lookahead_limit",         VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 255}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_lookahead_limit) },
 #endif
     { "ff_boost",                   VAR_UINT8 | PROFILE_VALUE,  .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },
 

src/main/cli/settings.h

@@ -136,6 +136,7 @@ typedef enum {
     TABLE_GYRO_FILTER_DEBUG,
     TABLE_POSITION_ALT_SOURCE,
     TABLE_DSHOT_BITBANG,
+    TABLE_INTERPOLATED_SP,
 
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;

src/main/fc/rc.c

@@ -58,6 +58,8 @@ typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCom
 static float rawSetpoint[XYZ_AXIS_COUNT];
 // Stick deflection [-1.0, 1.0] before RC-Smoothing is applied
 static float rawDeflection[XYZ_AXIS_COUNT];
+static float oldRcCommand[XYZ_AXIS_COUNT];
+static bool isDuplicateFrame;
 #endif
 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
 static float throttlePIDAttenuation;
@@ -124,6 +126,12 @@ float getRawDeflection(int axis)
 {
     return rawDeflection[axis];
 }
+
+bool rcIsDuplicateFrame()
+{
+    return isDuplicateFrame;
+}
+
 #endif
 
 #define THROTTLE_LOOKUP_LENGTH 12
@@ -188,6 +196,11 @@ float applyCurve(int axis, float deflection)
     return applyRates(axis, deflection, fabsf(deflection));
 }
 
+float getRcCurveSlope(int axis, float deflection)
+{
+    return (applyCurve(axis, deflection + 0.01f) - applyCurve(axis, deflection)) * 100.0f;
+}
+
 static void calculateSetpointRate(int axis)
 {
     float angleRate;
@@ -600,7 +613,12 @@ FAST_CODE void processRcCommand(void)
 
 #ifdef USE_INTERPOLATED_SP
     if (isRXDataNew) {
+        isDuplicateFrame = true;
         for (int i = FD_ROLL; i <= FD_YAW; i++) {
+            if (rcCommand[i] != oldRcCommand[i]) {
+                isDuplicateFrame = false;
+            }
+            oldRcCommand[i] = rcCommand[i];
             const float rcCommandf = rcCommand[i] / 500.0f;
             const float rcCommandfAbs = fabsf(rcCommandf);
             rawSetpoint[i] = applyRates(i, rcCommandf, rcCommandfAbs);

src/main/fc/rc.h

@@ -49,3 +49,5 @@ float getRawSetpoint(int axis);
 float getRawDeflection(int axis);
 float applyCurve(int axis, float deflection);
 uint32_t getRcFrameNumber();
+float getRcCurveSlope(int axis, float deflection);
+bool rcIsDuplicateFrame();

src/main/flight/interpolated_setpoint.c

@@ -28,89 +28,76 @@
 #include "fc/rc.h"
 #include "flight/interpolated_setpoint.h"
 
-static float projectedSetpoint[XYZ_AXIS_COUNT];
+static float setpointDeltaImpl[XYZ_AXIS_COUNT];
+static float prevSetpointDeltaImpl[XYZ_AXIS_COUNT];
+static float setpointDelta[XYZ_AXIS_COUNT];
+
+
 static float prevSetpointSpeed[XYZ_AXIS_COUNT];
 static float prevRawSetpoint[XYZ_AXIS_COUNT];
-static float prevRawDeflection[XYZ_AXIS_COUNT];
+static float prevSetpointAcceleration[XYZ_AXIS_COUNT];
-static uint16_t interpolationSteps[XYZ_AXIS_COUNT];
+
-static float setpointChangePerIteration[XYZ_AXIS_COUNT];
-static float deflectionChangePerIteration[XYZ_AXIS_COUNT];
-static float setpointReservoir[XYZ_AXIS_COUNT];
-static float deflectionReservoir[XYZ_AXIS_COUNT];
 
 // Configuration
-static float ffLookaheadLimit;
-static float ffSpread;
 static float ffMaxRateLimit[XYZ_AXIS_COUNT];
 static float ffMaxRate[XYZ_AXIS_COUNT];
 
 void interpolatedSpInit(const pidProfile_t *pidProfile) {
     const float ffMaxRateScale = pidProfile->ff_max_rate_limit * 0.01f;
-    ffLookaheadLimit = pidProfile->ff_lookahead_limit * 0.0001f;
-    ffSpread = pidProfile->ff_spread;
     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
         ffMaxRate[i] = applyCurve(i, 1.0f);
         ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale;
     }
 }
 
-FAST_CODE_NOINLINE float interpolatedSpApply(int axis, float pidFrequency, bool newRcFrame) {
+FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type) {
-    const float rawSetpoint = getRawSetpoint(axis);
-    const float rawDeflection = getRawDeflection(axis);
 
-    float pidSetpointDelta = 0.0f;
-    static int iterationsSinceLastUpdate[XYZ_AXIS_COUNT];
     if (newRcFrame) {
+        float rawSetpoint = getRawSetpoint(axis); 
+        
+        const float rxInterval = currentRxRefreshRate * 1e-6f;
+        const float rxRate = 1.0f / rxInterval;
 
-        setpointReservoir[axis] -= iterationsSinceLastUpdate[axis] * setpointChangePerIteration[axis];
+        const float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
-        deflectionReservoir[axis] -= iterationsSinceLastUpdate[axis] * deflectionChangePerIteration[axis];
+        const float setpointAcceleration = (setpointSpeed - prevSetpointSpeed[axis]) * pidGetDT();
-        iterationsSinceLastUpdate[axis] = 0;
+        const float setpointJerk = setpointAcceleration - prevSetpointAcceleration[axis];
-
+        
-        // get the number of interpolation steps either dynamically based on RX refresh rate
+        setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
-        // or manually based on ffSpread configuration property
+        
-        if (ffSpread) {
-            interpolationSteps[axis] = (uint16_t) ((ffSpread + 1.0f) * 0.001f * pidFrequency);
-        } else {
-            interpolationSteps[axis] = (uint16_t) ((currentRxRefreshRate + 1000) * pidFrequency * 1e-6f + 0.5f);
-        }
-
-        // interpolate stick deflection
-        deflectionReservoir[axis] += rawDeflection - prevRawDeflection[axis];
-        deflectionChangePerIteration[axis] = deflectionReservoir[axis] / interpolationSteps[axis];
-        const float projectedStickPos =
-            rawDeflection + deflectionChangePerIteration[axis] * pidFrequency * ffLookaheadLimit;
-        projectedSetpoint[axis] = applyCurve(axis, projectedStickPos);
-        prevRawDeflection[axis] = rawDeflection;
-
-        // apply linear interpolation on setpoint
-        setpointReservoir[axis] += rawSetpoint - prevRawSetpoint[axis];
         const float ffBoostFactor = pidGetFfBoostFactor();
+        float clip = 1.0f;
+        float boostAmount = 0.0f;
         if (ffBoostFactor != 0.0f) {
-            const float speed = rawSetpoint - prevRawSetpoint[axis];
+            if (pidGetJerkLimitInverse()) {
-            if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(3.0f * speed) > fabsf(prevSetpointSpeed[axis])) {
+                clip = 1 / (1 + fabsf(setpointJerk * pidGetJerkLimitInverse()));
-                const float setpointAcc = speed - prevSetpointSpeed[axis];
+                clip *= clip;
-                setpointReservoir[axis] += ffBoostFactor * setpointAcc;
+            }
+
+            // prevent kick-back spike at max deflection
+            if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || fabsf(setpointSpeed) > 3.0f * fabsf(prevSetpointSpeed[axis])) {
+                boostAmount = ffBoostFactor * setpointAcceleration;
             }
-            prevSetpointSpeed[axis] = speed;
         }
-
+        prevSetpointSpeed[axis] = setpointSpeed;
-        setpointChangePerIteration[axis] = setpointReservoir[axis] / interpolationSteps[axis];
+        prevSetpointAcceleration[axis] = setpointAcceleration;
-
         prevRawSetpoint[axis] = rawSetpoint;
         
         if (axis == FD_ROLL) {
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, rawDeflection * 100);
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, setpointDeltaImpl[axis] * 1000);
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, projectedStickPos * 100);
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, boostAmount * 1000);
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, projectedSetpoint[axis]);
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, boostAmount * clip * 1000);
+            DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, setpointJerk * 1000);
+        }
+        setpointDeltaImpl[axis] += boostAmount * clip;
+        if (type == FF_INTERPOLATE_ON) {
+            setpointDelta[axis] = setpointDeltaImpl[axis];
+        } else {
+            setpointDelta[axis] = 0.5f * (setpointDeltaImpl[axis] + prevSetpointDeltaImpl[axis]);
+            prevSetpointDeltaImpl[axis] = setpointDeltaImpl[axis];
         }
     }
-
+    
-    if (iterationsSinceLastUpdate[axis] < interpolationSteps[axis]) {
+    return setpointDelta[axis];
-        iterationsSinceLastUpdate[axis]++;
-        pidSetpointDelta = setpointChangePerIteration[axis];
-    }
-
-    return pidSetpointDelta;
 }
 
 FAST_CODE_NOINLINE float applyFfLimit(int axis, float value, float Kp, float currentPidSetpoint) {
@@ -118,13 +105,6 @@ FAST_CODE_NOINLINE float applyFfLimit(int axis, float value, float Kp, float cur
         DEBUG_SET(DEBUG_FF_LIMIT, 0, value);
     }
 
-    if (ffLookaheadLimit) {
-        const float limit = fabsf((projectedSetpoint[axis] - prevRawSetpoint[axis]) * Kp);
-        value = constrainf(value, -limit, limit);
-        if (axis == FD_ROLL) {
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, projectedSetpoint[axis]);
-        }
-    }
     if (axis == FD_ROLL) {
         DEBUG_SET(DEBUG_FF_LIMIT, 1, value);
     }
@@ -144,7 +124,7 @@ FAST_CODE_NOINLINE float applyFfLimit(int axis, float value, float Kp, float cur
 
 bool shouldApplyFfLimits(int axis)
 {
-    return ffLookaheadLimit != 0.0f || ffMaxRateLimit[axis] != 0.0f;
+    return ffMaxRateLimit[axis] != 0.0f;
 }
 
 

src/main/flight/interpolated_setpoint.h

@@ -25,7 +25,13 @@
 #include "common/axis.h"
 #include "flight/pid.h"
 
+typedef enum ffInterpolationType_e {
+    FF_INTERPOLATE_OFF,
+    FF_INTERPOLATE_ON,
+    FF_INTERPOLATE_AVG
+} ffInterpolationType_t;
+
 void interpolatedSpInit(const pidProfile_t *pidProfile);
-float interpolatedSpApply(int axis, float pidFrequency, bool newRcFrame);
+float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type);
 float applyFfLimit(int axis, float value, float Kp, float currentPidSetpoint);
 bool shouldApplyFfLimits(int axis);

src/main/flight/pid.c

@@ -130,7 +130,7 @@ static FAST_RAM_ZERO_INIT float airmodeThrottleOffsetLimit;
 
 #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, 12);
+PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 13);
 
 void resetPidProfile(pidProfile_t *pidProfile)
 {
@@ -212,10 +212,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .idle_p = 50,
         .idle_pid_limit = 200,
         .idle_max_increase = 150,
-        .ff_interpolate_sp = 0,
+        .ff_interpolate_sp = FF_INTERPOLATE_AVG,
-        .ff_spread = 0,
+        .ff_jerk_limit = 25,
-        .ff_max_rate_limit = 0,
+        .ff_max_rate_limit = 100,
-        .ff_lookahead_limit = 0,
         .ff_boost = 15,
     );
 #ifndef USE_D_MIN
@@ -315,6 +314,13 @@ 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 ffJerkLimitInverse;
+
+float pidGetJerkLimitInverse()
+{
+    return ffJerkLimitInverse;
+}
+
 
 float pidGetFfBoostFactor()
 {
@@ -460,6 +466,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
 
     ffBoostFactor = (float)pidProfile->ff_boost / 10.0f;
+    ffJerkLimitInverse = pidProfile->ff_jerk_limit ? 1.0f / ((float)pidProfile->ff_jerk_limit / 10.0f) : 0.0f;
 }
 
 #ifdef USE_RC_SMOOTHING_FILTER
@@ -583,7 +590,7 @@ static FAST_RAM_ZERO_INIT float dMinSetpointGain;
 #endif
 
 #ifdef USE_INTERPOLATED_SP
-static FAST_RAM_ZERO_INIT bool ffFromInterpolatedSetpoint;
+static FAST_RAM_ZERO_INIT ffInterpolationType_t ffFromInterpolatedSetpoint;
 #endif
 
 void pidInitConfig(const pidProfile_t *pidProfile)
@@ -1408,7 +1415,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         float pidSetpointDelta = 0;
 #ifdef USE_INTERPOLATED_SP
         if (ffFromInterpolatedSetpoint) {
-            pidSetpointDelta = interpolatedSpApply(axis, pidFrequency, newRcFrame);
+            pidSetpointDelta = interpolatedSpApply(axis, newRcFrame, ffFromInterpolatedSetpoint);
         } else {
             pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
         }

src/main/flight/pid.h

@@ -182,9 +182,8 @@ typedef struct pidProfile_s {
     uint8_t idle_max_increase;              // max integrated correction
     
     uint8_t ff_interpolate_sp;              // Calculate FF from interpolated setpoint
-    uint8_t ff_spread;                      // Spread ff out over at least min spread ms
     uint8_t ff_max_rate_limit;              // Maximum setpoint rate percentage for FF
-    uint8_t ff_lookahead_limit;             // FF stick extrapolation lookahead period in ms
+    uint8_t ff_jerk_limit;             // FF stick extrapolation lookahead period in ms
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -262,3 +261,4 @@ float pidGetPreviousSetpoint(int axis);
 float pidGetDT();
 float pidGetPidFrequency();
 float pidGetFfBoostFactor();
+float pidGetJerkLimitInverse();