From fde2aae4f5b6311dd7a3b918524fb7da5a9630fd Mon Sep 17 00:00:00 2001
From: Thorsten Laux <thorsten.laux@gmx.net>
Date: Thu, 24 May 2018 14:08:52 +0200
Subject: [PATCH] If P and FF have same sign take larger one

---
 src/main/flight/pid.c | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index f3ed603b2..40e06a849 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -627,9 +627,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             // This is done to avoid DTerm spikes that occur with dynamically
             // calculated deltaT whenever another task causes the PID
             // loop execution to be delayed.
-            const float delta = (
-                dynCd * transition * (currentPidSetpoint - previousPidSetpoint[axis]) -
-                (gyroRateDterm[axis] - previousGyroRateDterm[axis])) / dT;
+            const float delta =
+                - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) / dT;
 
             previousPidSetpoint[axis] = currentPidSetpoint;
             previousGyroRateDterm[axis] = gyroRateDterm[axis];
@@ -638,6 +637,17 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
 
             pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor;
 
+
+            const float pid_ff =
+                pidCoefficient[axis].Kd * dynCd * transition *
+                (currentPidSetpoint - previousPidSetpoint[axis]) * tpaFactor / dT;
+            if ((pidData[axis].P > 0) == (pid_ff > 0)) {
+                if (ABS(pid_ff) > ABS(pidData[axis].P)) {
+                    pidData[axis].P = 0;
+                    pidData[axis].D += pid_ff;
+                }
+            }
+
 #ifdef USE_YAW_SPIN_RECOVERY
             if (yawSpinActive)  {
                 // zero PIDs on pitch and roll leaving yaw P to correct spin 
@@ -649,6 +659,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
         }
     }
 
+    
     // calculating the PID sum
     pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D;
     pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D;