From fba0b2cea3f9fec1f6dc93f49c637f76b68cdb28 Mon Sep 17 00:00:00 2001
From: Bruce Luckcuck <github@etracer.net>
Date: Tue, 30 Jan 2018 09:58:41 -0500
Subject: [PATCH] Optimze pidSum calculation by storing the value rather than
 recalculating in multiple places.

Saves 72 bytes. Will save at least that additionally when incorporated into Runaway Takeoff Prevention (which also calculates the pidSum in two places).

Additionally adds a slight performance improvement by not repeating the floating point additions to calculate the pidSum in multiple places.  Effectively replaces 2 calculations with 1 (4 with 1 with Runaway Takeoff Prevention).
---
 src/main/flight/mixer.c  | 6 +++---
 src/main/flight/pid.c    | 6 +++++-
 src/main/flight/pid.h    | 1 +
 src/main/flight/servos.c | 6 +++---
 4 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index f179adab6e..12aa1a4142 100644
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -694,11 +694,11 @@ void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
 
     // Calculate and Limit the PIDsum
     const float scaledAxisPidRoll =
-        constrainf(axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
+        constrainf(axisPIDSum[FD_ROLL], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
     const float scaledAxisPidPitch =
-        constrainf(axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
+        constrainf(axisPIDSum[FD_PITCH], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
     float scaledAxisPidYaw =
-        constrainf(axisPID_P[FD_YAW] + axisPID_I[FD_YAW], -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING;
+        constrainf(axisPIDSum[FD_YAW], -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING;
     if (!mixerConfig()->yaw_motors_reversed) {
         scaledAxisPidYaw = -scaledAxisPidYaw;
     }
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 27b4fe6d43..5ab587d1d9 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -56,7 +56,7 @@ static FAST_RAM bool pidStabilisationEnabled;
 
 static FAST_RAM bool inCrashRecoveryMode = false;
 
-FAST_RAM float axisPID_P[3], axisPID_I[3], axisPID_D[3];
+FAST_RAM float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3];
 
 static FAST_RAM float dT;
 
@@ -530,6 +530,9 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
                 }
             }
             axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
+            axisPIDSum[axis] = axisPID_P[axis] + axisPID_I[axis] + axisPID_D[axis];
+        } else {
+            axisPIDSum[axis] = axisPID_P[axis] + axisPID_I[axis];
         }
 
         // Disable PID control if at zero throttle or if gyro overflow detected
@@ -537,6 +540,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
             axisPID_P[axis] = 0;
             axisPID_I[axis] = 0;
             axisPID_D[axis] = 0;
+            axisPIDSum[axis] = 0;
         }
     }
 }
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index ca032d7904..7f536139c3 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -121,6 +121,7 @@ union rollAndPitchTrims_u;
 void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, timeUs_t currentTimeUs);
 
 extern float axisPID_P[3], axisPID_I[3], axisPID_D[3];
+extern float axisPIDSum[3];
 bool airmodeWasActivated;
 extern uint32_t targetPidLooptime;
 
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index a1947d2dab..1e2cc2a8fd 100644
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -401,9 +401,9 @@ void servoMixer(void)
         input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
     } else {
         // 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_ROLL] = axisPIDSum[FD_ROLL] * PID_SERVO_MIXER_SCALING;
+        input[INPUT_STABILIZED_PITCH] = axisPIDSum[FD_PITCH] * PID_SERVO_MIXER_SCALING;
+        input[INPUT_STABILIZED_YAW] = axisPIDSum[FD_YAW] * PID_SERVO_MIXER_SCALING;
 
         // Reverse yaw servo when inverted in 3D mode
         if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) {