Improved ITerm windup handling for tricopter

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) {

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);

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