From c55d0baf31e66845316837df995eba552b974fc0 Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Thu, 4 Feb 2016 01:55:44 +0100
Subject: [PATCH] Fix non Working Anti Windup

---
 src/main/flight/pid.c    | 26 +++++++++-----------------
 src/main/flight/pid.h    |  3 +--
 src/main/io/serial_msp.c |  2 +-
 src/main/mw.c            | 11 ++++++++++-
 4 files changed, 21 insertions(+), 21 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index dadc564919..fcb094f14e 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -50,6 +50,7 @@
 
 extern float dT;
 extern bool motorLimitReached;
+bool lowThrottleWindupProtection;
 
 #define PREVENT_WINDUP(x,y) { if (ABS(x) > ABS(y)) { if (x < 0) { x = -ABS(y); }  else { x = ABS(y); } } }
 
@@ -75,21 +76,12 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
 
 pidControllerFuncPtr pid_controller = pidRewrite; // which pid controller are we using, defaultMultiWii
 
-void pidResetErrorGyro(rxConfig_t *rxConfig)
+void pidResetErrorGyro(void)
 {
     int axis;
-
     for (axis = 0; axis < 3; axis++) {
-        if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
-            int rollPitchStatus = calculateRollPitchCenterStatus(rxConfig);
-            if (rollPitchStatus == CENTERED) {
-                PREVENT_WINDUP(errorGyroI[axis], previousErrorGyroI[axis]);
-                PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
-            }
-        } else {
-            errorGyroI[axis] = 0;
-            errorGyroIf[axis] = 0.0f;
-        }
+        errorGyroI[axis] = 0;
+        errorGyroIf[axis] = 0.0f;
     }
 }
 
@@ -208,9 +200,9 @@ 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 (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
+        if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
             errorGyroIf[axis] *= scaleItermToRcInput(axis);
-            if (motorLimitReached) {
+            if (motorLimitReached || lowThrottleWindupProtection) {
                 PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
             } else {
                 previousErrorGyroIf[axis] = errorGyroIf[axis];
@@ -355,10 +347,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         // I coefficient (I8) moved before integration to make limiting independent from PID settings
         errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
 
-        if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
+        if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
             errorGyroI[axis] = (int32_t) (errorGyroI[axis] * scaleItermToRcInput(axis));
-            if (motorLimitReached) {
-                PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
+            if (motorLimitReached || lowThrottleWindupProtection) {
+                PREVENT_WINDUP(errorGyroI[axis], previousErrorGyroI[axis]);
             } else {
                 previousErrorGyroI[axis] = errorGyroI[axis];
             }
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index f9c6f5d3f5..cbfa9af3c9 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -21,7 +21,6 @@
 #define GYRO_I_MAX 256                      // Gyro I limiter
 #define YAW_P_LIMIT_MIN 100                 // Maximum value for yaw P limiter
 #define YAW_P_LIMIT_MAX 500                 // Maximum value for yaw P limiter
-#define IS_POSITIVE(x) ((x > 0) ? true : false)
 
 typedef enum {
     PIDROLL,
@@ -81,5 +80,5 @@ extern int16_t axisPID[XYZ_AXIS_COUNT];
 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 
 void pidSetController(pidControllerType_e type);
-void pidResetErrorGyro(rxConfig_t *rxConfig);
+void pidResetErrorGyro(void);
 
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 6a81edc419..38f1b2e257 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -103,7 +103,7 @@ extern void resetPidProfile(pidProfile_t *pidProfile);
 
 void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
 
-const char * const flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
+const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 
 typedef struct box_e {
     const uint8_t boxId;         // see boxId_e
diff --git a/src/main/mw.c b/src/main/mw.c
index 78c671bc72..7e139c9747 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -117,6 +117,8 @@ static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the m
 
 extern uint32_t currentTime;
 extern uint8_t PIDweight[3];
+extern bool lowThrottleWindupProtection;
+
 
 static bool isRXDataNew;
 
@@ -476,9 +478,16 @@ void processRx(void)
     }
 
     throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
+    rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig);
 
     if (throttleStatus == THROTTLE_LOW) {
-        pidResetErrorGyro(&masterConfig.rxConfig);
+        if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && rollPitchStatus == CENTERED) { /* Anti Windup when roll / pitch stick centered */
+            lowThrottleWindupProtection = true;
+        } else {
+            pidResetErrorGyro();
+        }
+    } else {
+        lowThrottleWindupProtection = false;
     }
 
     // When armed and motors aren't spinning, do beeps and then disarm