fixes for feedforward for 4.3

src/main/build/debug.c

@@ -91,8 +91,8 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "BARO",
     "GPS_RESCUE_THROTTLE_PID",
     "DYN_IDLE",
-    "FF_LIMIT",
+    "FEEDFORWARD_LIMIT",
-    "FF_INTERPOLATED",
+    "FEEDFORWARD",
     "BLACKBOX_OUTPUT",
     "GYRO_SAMPLE",
     "RX_TIMING",

src/main/build/debug.h

@@ -107,8 +107,8 @@ typedef enum {
     DEBUG_BARO,
     DEBUG_GPS_RESCUE_THROTTLE_PID,
     DEBUG_DYN_IDLE,
-    DEBUG_FF_LIMIT,
+    DEBUG_FEEDFORWARD_LIMIT,
-    DEBUG_FF_INTERPOLATED,
+    DEBUG_FEEDFORWARD,
     DEBUG_BLACKBOX_OUTPUT,
     DEBUG_GYRO_SAMPLE,
     DEBUG_RX_TIMING,

src/main/cli/settings.c

@@ -1151,7 +1151,7 @@ const clivalue_t valueTable[] = {
 
 #ifdef USE_FEEDFORWARD
     { "feedforward_averaging",        VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FEEDFORWARD_AVERAGING }, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_averaging) },
-    { "feedforward_smooth_factor",    VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) },
+    { "feedforward_smoothing",        VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_smooth_factor) },
     { "feedforward_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_jitter_factor) },
     { "feedforward_max_rate_limit",   VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, feedforward_max_rate_limit) },
 #endif

src/main/fc/rc.c

@@ -554,7 +554,7 @@ FAST_CODE void processRcCommand(void)
         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 
             isDuplicate[axis] = (oldRcCommand[axis] == rcCommand[axis]);
-            rcCommandDelta[axis] = fabsf(rcCommand[axis] - oldRcCommand[axis]);
+            rcCommandDelta[axis] = (rcCommand[axis] - oldRcCommand[axis]);
             oldRcCommand[axis] = rcCommand[axis];
 
             float angleRate;

src/main/flight/feedforward.c

@@ -33,7 +33,6 @@
 
 #include "feedforward.h"
 
-static float setpointDeltaImpl[XYZ_AXIS_COUNT];
 static float setpointDelta[XYZ_AXIS_COUNT];
 
 typedef struct laggedMovingAverageCombined_s {
@@ -43,23 +42,21 @@ typedef struct laggedMovingAverageCombined_s {
 
 laggedMovingAverageCombined_t  setpointDeltaAvg[XYZ_AXIS_COUNT];
 
-static float prevSetpoint[XYZ_AXIS_COUNT]; // equals raw unless interpolated 
+static float prevSetpoint[XYZ_AXIS_COUNT];
-static float prevSetpointSpeed[XYZ_AXIS_COUNT]; // equals raw unless interpolated
+static float prevSetpointSpeed[XYZ_AXIS_COUNT];
-static float prevAcceleration[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation
+static float prevAcceleration[XYZ_AXIS_COUNT];
-static float prevRcCommandDelta[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation
+static float prevRcCommandDelta[XYZ_AXIS_COUNT];
-
+static bool prevDuplicatePacket[XYZ_AXIS_COUNT];
-static bool prevDuplicatePacket[XYZ_AXIS_COUNT]; // to identify multiple identical packets
 static uint8_t averagingCount;
-
+static float feedforwardMaxRateLimit[XYZ_AXIS_COUNT];
-static float ffMaxRateLimit[XYZ_AXIS_COUNT];
+static float feedforwardMaxRate[XYZ_AXIS_COUNT];
-static float ffMaxRate[XYZ_AXIS_COUNT];
 
 void feedforwardInit(const pidProfile_t *pidProfile) {
-    const float ffMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f;
+    const float feedforwardMaxRateScale = pidProfile->feedforward_max_rate_limit * 0.01f;
     averagingCount = pidProfile->feedforward_averaging + 1;
     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
-        ffMaxRate[i] = applyCurve(i, 1.0f);
+        feedforwardMaxRate[i] = applyCurve(i, 1.0f);
-        ffMaxRateLimit[i] = ffMaxRate[i] * ffMaxRateScale;
+        feedforwardMaxRateLimit[i] = feedforwardMaxRate[i] * feedforwardMaxRateScale;
         laggedMovingAverageInit(&setpointDeltaAvg[i].filter, averagingCount, (float *)&setpointDeltaAvg[i].buf[0]);
     }
 }
@@ -70,88 +67,96 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward
         float rcCommandDelta = getRcCommandDelta(axis);
         float setpoint = getRawSetpoint(axis);
         const float rxInterval = getCurrentRxRefreshRate() * 1e-6f;
-        const float rxRate = 1.0f / rxInterval;
+        const float rxRate = 1.0f / rxInterval; // eg 150 for a 150Hz RC link
         float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
         float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
         float setpointAcceleration = 0.0f;
-        const float ffSmoothFactor = pidGetFfSmoothFactor();
+        const float feedforwardSmoothFactor = pidGetFeedforwardSmoothFactor();
-        const float ffJitterFactor = pidGetFfJitterFactor();
+        const float feedforwardJitterFactor = pidGetFeedforwardJitterFactor();
+        float feedforward;
 
         // calculate an attenuator from average of two most recent rcCommand deltas vs jitter threshold
-        float ffAttenuator = 1.0f;
+        if (axis == FD_ROLL) {
-        if (ffJitterFactor) {
+            DEBUG_SET(DEBUG_FEEDFORWARD, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference = steps of 50 mean 2000 RC steps
-            if (rcCommandDelta < ffJitterFactor) {
+        }
-                ffAttenuator = MAX(1.0f - ((rcCommandDelta + prevRcCommandDelta[axis]) / 2.0f) / ffJitterFactor, 0.0f);
+        rcCommandDelta = fabsf(rcCommandDelta);
-                ffAttenuator = 1.0f - ffAttenuator * ffAttenuator;
+        float jitterAttenuator = 1.0f;
+        if (feedforwardJitterFactor) {
+            if (rcCommandDelta < feedforwardJitterFactor) {
+                jitterAttenuator = MAX(1.0f - ((rcCommandDelta + prevRcCommandDelta[axis]) / 2.0f) / feedforwardJitterFactor, 0.0f);
+                jitterAttenuator = 1.0f - jitterAttenuator * jitterAttenuator;
             }
         }
 
+        const float setpointPercent = fabsf(setpoint) / feedforwardMaxRate[axis];
+        float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
+
         // interpolate setpoint if necessary
         if (rcCommandDelta == 0.0f) {
-            if (prevDuplicatePacket[axis] == false && fabsf(setpoint) < 0.98f * ffMaxRate[axis]) {
+            if (prevDuplicatePacket[axis] == false) {
-                // first duplicate after movement
+                // first duplicate after movement, interpolate setpoint and use previous acceleration
-                // interpolate rawSetpoint by adding (speed + acceleration) * attenuator to previous setpoint
+                // don't interpolate if sticks close to centre or max, interpolate jitter signals less than larger ones
-                setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis]) * ffAttenuator * rxInterval;
+                if (setpointPercent > 0.02f && setpointPercent < 0.95f) {
-                // recalculate setpointSpeed and (later) acceleration from this new setpoint value
+                    // setpoint interpolation includes previous acceleration and attenuation
-                setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
+                    setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis] * jitterAttenuator ) * rxInterval * jitterAttenuator;
+                    // recalculate speed and acceleration
+                    setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
+                }
+            } else {
+                // force to zero
+                setpointSpeed = 0.0f;
             }
             prevDuplicatePacket[axis] = true;
         } else {
-            // movement!
-            if (prevDuplicatePacket[axis] == true) {
-                // don't boost the packet after a duplicate, the feedforward alone is enough, usually
-                // in part because after a duplicate, the raw up-step is large, so the jitter attenuator is less active
-                ffAttenuator = 0.0f;
-            }
             prevDuplicatePacket[axis] = false;
         }
+
         prevSetpoint[axis] = setpoint;
 
         if (axis == FD_ROLL) {
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(setpoint)); // setpoint after interpolations
+            DEBUG_SET(DEBUG_FEEDFORWARD, 0, lrintf(setpoint)); // setpoint after interpolations
         }
 
-        float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
+        // first order type smoothing for derivative
+        setpointSpeed = prevSetpointSpeed[axis] + feedforwardSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
 
-        // calculate acceleration, smooth and attenuate it
+        // second order smoothing for for acceleration by calculating it after smoothing setpointSpeed
         setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
-        setpointAcceleration = prevAcceleration[axis] + ffSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
+        setpointAcceleration *= rxRate * 0.01f; // adjust boost for RC packet interval, including dropped packets
-        setpointAcceleration *= ffAttenuator;
+        setpointAcceleration = prevAcceleration[axis] + feedforwardSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
-
-        // smooth setpointSpeed but don't attenuate
-        setpointSpeed = prevSetpointSpeed[axis] + ffSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
 
         prevSetpointSpeed[axis] = setpointSpeed;
         prevAcceleration[axis] = setpointAcceleration;
         prevRcCommandDelta[axis] = rcCommandDelta;
 
         setpointAcceleration *= pidGetDT();
-        setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
+        feedforward = setpointSpeed * pidGetDT();
 
         // calculate boost and prevent kick-back spike at max deflection
-        const float ffBoostFactor = pidGetFfBoostFactor();
+        const float feedforwardBoostFactor = pidGetFeedforwardBoostFactor();
         float boostAmount = 0.0f;
-        if (ffBoostFactor) {
+        if (feedforwardBoostFactor) {
-            if (fabsf(setpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
+            // allows boost when returning from max, but not when hitting max on the way up
-                boostAmount = ffBoostFactor * setpointAcceleration;
+            if (setpointPercent < 0.95f || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
+                boostAmount = feedforwardBoostFactor * setpointAcceleration * jitterAttenuator;
             }
         }
 
         if (axis == FD_ROLL) {
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100.0f)); // base feedforward
+            DEBUG_SET(DEBUG_FEEDFORWARD, 1, lrintf(feedforward * 100.0f)); // delta after interpolating duplicates and smoothing
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(boostAmount * 100.0f)); // boost amount
+            DEBUG_SET(DEBUG_FEEDFORWARD, 2, lrintf(boostAmount * 100.0f)); // boost amount after jitter reduction and smoothing
-            // debug 2 is interpolated setpoint, above
+            // debug 0 is interpolated setpoint, above
-            DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference
+            // debug 3 is rcCommand delta, above
         }
 
-        // add boost to base feedforward
+        // add attenuated boost to base feedforward
-        setpointDeltaImpl[axis] += boostAmount;
+        feedforward += boostAmount;
 
-        // apply averaging
+        // apply averaging, if enabled
         if (feedforwardAveraging) {
-            setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, setpointDeltaImpl[axis]);
+            setpointDelta[axis] = laggedMovingAverageUpdate(&setpointDeltaAvg[axis].filter, feedforward);
         } else {
-            setpointDelta[axis] = setpointDeltaImpl[axis];
+            setpointDelta[axis] = feedforward;
         }
     }
     return setpointDelta[axis];
@@ -160,23 +165,23 @@ FAST_CODE_NOINLINE float feedforwardApply(int axis, bool newRcFrame, feedforward
 FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp, float currentPidSetpoint) {
     switch (axis) {
     case FD_ROLL:
-        DEBUG_SET(DEBUG_FF_LIMIT, 0, value);
+        DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 0, value);
-
         break;
     case FD_PITCH:
-        DEBUG_SET(DEBUG_FF_LIMIT, 1, value);
+        DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 1, value);
-
         break;
     }
 
-    if (fabsf(currentPidSetpoint) <= ffMaxRateLimit[axis]) {
+    if (value * currentPidSetpoint > 0.0f) {
-        value = constrainf(value, (-ffMaxRateLimit[axis] - currentPidSetpoint) * Kp, (ffMaxRateLimit[axis] - currentPidSetpoint) * Kp);
+        if (fabsf(currentPidSetpoint) <= feedforwardMaxRateLimit[axis]) {
-    } else {
+            value = constrainf(value, (-feedforwardMaxRateLimit[axis] - currentPidSetpoint) * Kp, (feedforwardMaxRateLimit[axis] - currentPidSetpoint) * Kp);
-        value = 0;
+        } else {
+            value = 0;
+        }
     }
 
     if (axis == FD_ROLL) {
-        DEBUG_SET(DEBUG_FF_LIMIT, 2, value);
+        DEBUG_SET(DEBUG_FEEDFORWARD_LIMIT, 2, value);
     }
 
     return value;
@@ -184,6 +189,6 @@ FAST_CODE_NOINLINE float applyFeedforwardLimit(int axis, float value, float Kp,
 
 bool shouldApplyFeedforwardLimits(int axis)
 {
-    return ffMaxRateLimit[axis] != 0.0f && axis < FD_YAW;
+    return feedforwardMaxRateLimit[axis] != 0.0f && axis < FD_YAW;
 }
 #endif

src/main/flight/pid.c

@@ -204,7 +204,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .dyn_idle_max_increase = 150,
         .feedforward_averaging = FEEDFORWARD_AVERAGING_OFF,
         .feedforward_max_rate_limit = 100,
-        .feedforward_smooth_factor = 37,
+        .feedforward_smooth_factor = 25,
         .feedforward_jitter_factor = 7,
         .feedforward_boost = 15,
         .dyn_lpf_curve_expo = 5,
@@ -256,20 +256,20 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState)
 
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
-float pidGetFfBoostFactor()
+float pidGetFeedforwardBoostFactor()
 {
-    return pidRuntime.ffBoostFactor;
+    return pidRuntime.feedforwardBoostFactor;
 }
 
 #ifdef USE_FEEDFORWARD
-float pidGetFfSmoothFactor()
+float pidGetFeedforwardSmoothFactor()
 {
-    return pidRuntime.ffSmoothFactor;
+    return pidRuntime.feedforwardSmoothFactor;
 }
 
-float pidGetFfJitterFactor()
+float pidGetFeedforwardJitterFactor()
 {
-    return pidRuntime.ffJitterFactor;
+    return pidRuntime.feedforwardJitterFactor;
 }
 #endif
 

src/main/flight/pid.h

@@ -284,7 +284,7 @@ typedef struct pidRuntime_s {
     float antiGravityOsdCutoff;
     float antiGravityThrottleHpf;
     float antiGravityPBoost;
-    float ffBoostFactor;
+    float feedforwardBoostFactor;
     float itermAccelerator;
     uint16_t itermAcceleratorGain;
     float feedforwardTransition;
@@ -386,8 +386,8 @@ typedef struct pidRuntime_s {
 
 #ifdef USE_FEEDFORWARD
     feedforwardAveraging_t feedforwardAveraging;
-    float ffSmoothFactor;
+    float feedforwardSmoothFactor;
-    float ffJitterFactor;
+    float feedforwardJitterFactor;
 #endif
 } pidRuntime_t;
 
@@ -440,7 +440,7 @@ void pidSetItermReset(bool enabled);
 float pidGetPreviousSetpoint(int axis);
 float pidGetDT();
 float pidGetPidFrequency();
-float pidGetFfBoostFactor();
+float pidGetFeedforwardBoostFactor();
-float pidGetFfSmoothFactor();
+float pidGetFeedforwardSmoothFactor();
-float pidGetFfJitterFactor();
+float pidGetFeedforwardJitterFactor();
 float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);

src/main/flight/pid_init.c

@@ -239,7 +239,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT));
     pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT));
 
-    pidRuntime.ffBoostFactor = (float)pidProfile->feedforward_boost / 10.0f;
+    pidRuntime.feedforwardBoostFactor = (float)pidProfile->feedforward_boost / 10.0f;
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -423,13 +423,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
 
 #ifdef USE_FEEDFORWARD
     pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging;
+    pidRuntime.feedforwardSmoothFactor = 1.0f;
     if (pidProfile->feedforward_smooth_factor) {
-        pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f;
+        pidRuntime.feedforwardSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f;
-    } else {
-        // set automatically according to boost amount, limit to 0.5 for auto
-        pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->feedforward_boost) * 2.0f / 100.0f);
     }
-    pidRuntime.ffJitterFactor = pidProfile->feedforward_jitter_factor;
+    pidRuntime.feedforwardJitterFactor = pidProfile->feedforward_jitter_factor;
     feedforwardInit(pidProfile);
 #endif