From a5872041cc192896794839ba0a6098300baba35d Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Fri, 31 Mar 2017 16:37:10 +0200
Subject: [PATCH] Cleanup pidsum code

---
 src/main/flight/mixer.c  |  2 +-
 src/main/flight/pid.c    | 17 +++++++++++------
 src/main/flight/servos.c |  2 +-
 3 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 6f3847c02..772553f69 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -487,7 +487,7 @@ void mixTable(pidProfile_t *pidProfile)
         constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING,
         -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
     scaledAxisPIDf[FD_YAW] =
-        constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING,
+        constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW] + axisPID_D[FD_YAW]) / PID_MIXER_SCALING,
         -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw);
 
     // Calculate voltage compensation
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 8e4d939c8..ce928089d 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -258,7 +258,12 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
         }
 
         // -----calculate D component
-        if (axis != FD_YAW) {
+        if (axis == FD_YAW) {
+            // no DTerm for yaw axis
+            axisPID_P[FD_YAW] = PTerm;
+            axisPID_I[FD_YAW] = ITerm;
+            axisPID_D[FD_YAW] = 0;
+        } else {
             float dynC = dtermSetpointWeight;
             if (pidProfile->setpointRelaxRatio < 100) {
                 dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
@@ -276,11 +281,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
         }
 
-        // Enable PID control only when stabilisation on
-        if (pidStabilisationEnabled) {
-            axisPID_P[axis] = PTerm;
-            axisPID_I[axis] = ITerm;
-            axisPID_D[axis] = DTerm;
+        // Disable PID control at zero throttle
+        if (!pidStabilisationEnabled) {
+            axisPID_P[axis] = 0;
+            axisPID_I[axis] = 0;
+            axisPID_D[axis] = 0;
         }
     }
 }
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index 7e4f3464b..2092bac28 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -345,7 +345,7 @@ STATIC_UNIT_TESTED void servoMixer(void)
         // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
         input[INPUT_STABILIZED_ROLL] = (axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) * PID_SERVO_MIXER_SCALING;
         input[INPUT_STABILIZED_PITCH] = (axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) * PID_SERVO_MIXER_SCALING;
-        input[INPUT_STABILIZED_YAW] = (axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) * PID_SERVO_MIXER_SCALING;
+        input[INPUT_STABILIZED_YAW] = (axisPID_P[FD_YAW] + axisPID_I[FD_YAW] + axisPID_D[FD_PITCH]) * PID_SERVO_MIXER_SCALING;
 
         // Reverse yaw servo when inverted in 3D mode
         if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {