From 9ec26626b768170da9975a87839a477e80f8e67a Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Wed, 9 Dec 2015 13:08:00 +0100
Subject: [PATCH] Accumulation Prevention in Saturation for AIR Mode //
 Refactoring

Adjust shrink threshold for errorRatio
---
 src/main/flight/mixer.c | 35 +++++++++++++++++------------------
 src/main/flight/pid.c   | 10 +++++-----
 2 files changed, 22 insertions(+), 23 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index c6d5504c1..f54894ff9 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -69,8 +69,7 @@ static rxConfig_t *rxConfig;
 static mixerMode_e currentMixerMode;
 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
 
-float rpy_limiting = 1.0f;
-
+float totalErrorRatioLimit = 1.0f;
 
 #ifdef USE_SERVOS
 static uint8_t servoRuleCount = 0;
@@ -761,34 +760,34 @@ void mixTable(void)
                 -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
         }
     } else {
-        int16_t rpy[MAX_SUPPORTED_MOTORS];
-        int16_t rpy_max = 0; // assumption: symetrical about zero.
-        int16_t rpy_min = 0;
+        int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
+        int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
+        int16_t rollPitchYawMixMin = 0;
 
         // Find roll/pitch/yaw desired output
         for (i = 0; i < motorCount; i++) {
-            rpy[i] =
+            rollPitchYawMix[i] =
                 axisPID[PITCH] * currentMixer[i].pitch +
                 axisPID[ROLL] * currentMixer[i].roll +
                 -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
 
-            if (rpy[i] > rpy_max) rpy_max = rpy[i];
-            if (rpy[i] < rpy_min) rpy_min = rpy[i];
+            if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
+            if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
         }
 
         // Scale roll/pitch/yaw uniformly to fit within throttle range
-        int16_t rpy_range = rpy_max - rpy_min;
-        int16_t th_range = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle;
-        int16_t th_min, th_max;
+        int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin;
+        int16_t throttleRange = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle;
+        int16_t throttleMin, throttleMax;
 
-        if (rpy_range > th_range) {
+        if (rollPitchYawMixRange > throttleRange) {
             for (i = 0; i < motorCount; i++) {
-                rpy[i] = (rpy[i] * th_range) / rpy_range;
+                rollPitchYawMix[i] = (rollPitchYawMix[i] * throttleRange) / rollPitchYawMixRange;
             }
-            th_min = th_max = escAndServoConfig->minthrottle + (th_range / 2);
+            throttleMin = throttleMax = escAndServoConfig->minthrottle + (throttleRange / 2);
         } else {
-            th_min = escAndServoConfig->minthrottle + (rpy_range / 2);
-            th_max = escAndServoConfig->maxthrottle - (rpy_range / 2);
+            throttleMin = escAndServoConfig->minthrottle + (rollPitchYawMixRange / 2);
+            throttleMax = escAndServoConfig->maxthrottle - (rollPitchYawMixRange / 2);
         }
 
         // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
@@ -797,11 +796,11 @@ void mixTable(void)
         // TODO: handle the case when motors don't all get the same throttle factor...
         //       too lazy to sort out the math right now.
         for (i = 0; i < motorCount; i++) {
-            motor[i] = rpy[i] + constrainf(rcCommand[THROTTLE] * currentMixer[i].throttle, th_min, th_max);
+            motor[i] = rollPitchYawMix[i] + constrainf(rcCommand[THROTTLE] * currentMixer[i].throttle, throttleMin, throttleMax);
         }
 
         // adjust feedback to scale PID error inputs to our limitations.
-        rpy_limiting = constrainf(((float)th_range / rpy_range), 0.1f, 1.0f);
+        totalErrorRatioLimit = constrainf(((float)throttleRange / rollPitchYawMixRange), 0.1f, 1.0f);
     }
 
     if (ARMING_FLAG(ARMED)) {
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 568b1ae28..4169cca1a 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -48,7 +48,7 @@
 #include "config/runtime_config.h"
 
 extern float dT;
-extern float rpy_limiting;
+extern float totalErrorRatioLimit;
 extern bool allowITermShrinkOnly;
 
 #define CALC_OFFSET(x) ( (x > 0) ?  x : -x )
@@ -163,7 +163,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         RateError = AngleRate - gyroRate;
 
         if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
-        	RateError = RateError * rpy_limiting;
+            RateError = RateError * totalErrorRatioLimit;
         }
 
         // -----calculate P component
@@ -176,7 +176,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // -----calculate I component.
         errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
 
-        if (allowITermShrinkOnly) {
+        if (allowITermShrinkOnly || totalErrorRatioLimit < 0.98f) {
             if (CALC_OFFSET(errorGyroIf[axis]) < CALC_OFFSET(previousErrorGyroIf[axis])) {
                 previousErrorGyroIf[axis] = errorGyroIf[axis];
             } else {
@@ -294,7 +294,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         RateError = AngleRateTmp - (gyroADC[axis] / 4);
 
         if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
-            RateError = RateError * rpy_limiting;
+            RateError = RateError * totalErrorRatioLimit;
         }
 
         // -----calculate P component
@@ -317,7 +317,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
 
         ITerm = errorGyroI[axis] >> 13;
 
-        if (allowITermShrinkOnly) {
+        if (allowITermShrinkOnly || totalErrorRatioLimit < 0.98f) {
             if (CALC_OFFSET(errorGyroI[axis]) < CALC_OFFSET(previousErrorGyroI[axis])) {
                 previousErrorGyroI[axis] = errorGyroI[axis];
             } else {