From d63eecffaac4bb432fb250ee9510a2800eea4910 Mon Sep 17 00:00:00 2001
From: Miguel Angel Mulero Martinez <migmul@gmail.com>
Date: Fri, 15 Jun 2018 15:08:09 +0200
Subject: [PATCH] Add Iterm Relax INC modes

---
 src/main/flight/pid.c         | 21 +++++++++++++--------
 src/main/flight/pid.h         |  4 +++-
 src/main/interface/settings.c |  2 +-
 3 files changed, 17 insertions(+), 10 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 7c2ab9e07..4342cde88 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -809,19 +809,27 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         float acCorrection = 0;
         float acErrorRate;
 #endif        
-        float itermErrorRate = 0.0f;
+
+        const float ITerm = pidData[axis].I;
+        float itermErrorRate = currentPidSetpoint - gyroRate;
 
 #if defined(USE_ITERM_RELAX)
-        if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
+        if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC)) {
             const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
             const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
             const float itermRelaxFactor = 1 - setpointHpf / 30.0f;
-            if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 30) {
-                itermErrorRate = itermRelaxFactor * (currentPidSetpoint - gyroRate);
+
+            const bool isDecreasingI = ((ITerm > 0) && (itermErrorRate < 0)) || ((ITerm < 0) && (itermErrorRate > 0));
+            if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
+                // Do Nothing, use the precalculed itermErrorRate
+            } else if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 30) {
+                itermErrorRate *= itermRelaxFactor;
             } else if (itermRelaxType == ITERM_RELAX_GYRO ) {
                 itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
+            } else {
+                itermErrorRate = 0.0f;
             }
-            
+
             if (axis == FD_ROLL) {
                 DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf));
                 DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
@@ -849,7 +857,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         } else
 #endif // USE_ITERM_RELAX
         {
-            itermErrorRate = currentPidSetpoint - gyroRate;
 #if defined(USE_ABSOLUTE_CONTROL)
             acErrorRate = itermErrorRate;
 #endif // USE_ABSOLUTE_CONTROL
@@ -883,8 +890,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         }
 
         // -----calculate I component
-
-        const float ITerm = pidData[axis].I;
         const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
         const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
         if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index d99e87082..4f4fe3d1c 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -79,7 +79,9 @@ typedef struct pid8_s {
 typedef enum {
     ITERM_RELAX_OFF,
     ITERM_RELAX_RP,
-    ITERM_RELAX_RPY
+    ITERM_RELAX_RPY,
+    ITERM_RELAX_RP_INC,
+    ITERM_RELAX_RPY_INC
 } itermRelax_e;
 
 typedef enum {
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index 45fb4cd34..0e4493169 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -339,7 +339,7 @@ static const char * const lookupTableVideoSystem[] = {
 
 #if defined(USE_ITERM_RELAX)
 static const char * const lookupTableItermRelax[] = {
-    "OFF", "RP", "RPY"
+    "OFF", "RP", "RPY", "RP_INC", "RPY_INC"
 };
 static const char * const lookupTableItermRelaxType[] = {
     "GYRO", "SETPOINT"