From 37c9d3c47edd2ae06924c9a828b1c8ee70dd6514 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Sat, 11 Mar 2017 07:44:24 +0000
Subject: [PATCH] Improved ITerm windup handling for tricopter

---
 src/main/flight/mixer.c | 13 +++++++++++++
 src/main/flight/mixer.h |  1 +
 src/main/flight/pid.c   |  9 ++++++---
 3 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 9f614dd6c..3840c45dc 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -116,6 +116,9 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR
 #define EXTERNAL_CONVERSION_MAX_VALUE 2000
 #define EXTERNAL_CONVERSION_3D_MID_VALUE 1500
 
+#define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight
+
+
 static uint8_t motorCount;
 static float motorMixRange;
 
@@ -328,6 +331,16 @@ float getMotorMixRange()
     return motorMixRange;
 }
 
+bool mixerIsOutputSaturated(int axis, float errorRate)
+{
+    if (axis == FD_YAW && (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI)) {
+        return errorRate > TRICOPTER_ERROR_RATE_YAW_SATURATED;
+    } else {
+        return motorMixRange >= 1.0f;
+    }
+    return false;
+}
+
 bool isMotorProtocolDshot(void) {
 #ifdef USE_DSHOT
     switch(motorConfig()->dev.motorPwmProtocol) {
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index d370d85d0..ad6bcdb1c 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -124,6 +124,7 @@ struct rxConfig_s;
 
 uint8_t getMotorCount();
 float getMotorMixRange();
+bool mixerIsOutputSaturated(int axis, float errorRate);
 
 void mixerLoadMix(int index, motorMixer_t *customMixers);
 void mixerInit(mixerMode_e mixerMode);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 7452a5a91..002bec1d8 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -335,9 +335,12 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
         }
 
         // -----calculate I component
-        if (motorMixRange < 1.0f) {
-            // Only increase ITerm if motor output is not saturated
-            axisPID_I[axis] += Ki[axis] * errorRate * dT * dynKi * itermAccelerator;
+        const float ITerm = axisPID_I[axis];
+        const float ITermNew = ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator;
+        const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
+        if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
+            // Only increase ITerm if output is not saturated
+            axisPID_I[axis] = ITermNew;
         }
 
         // -----calculate D component