Iterm Shrink Only Option for AIR Mode

Fix previous Iterm

src/main/flight/pid.c

@@ -49,6 +49,9 @@
 
 extern float dT;
 extern float rpy_limiting;
+extern bool allowITermShrinkOnly;
+
+#define CALC_OFFSET(x) ( (x > 0) ?  x : -x )
 
 int16_t axisPID[3];
 
@@ -96,6 +99,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     float delta;
     int axis;
     float horizonLevelStrength = 1;
+    static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
 
     if (FLIGHT_MODE(HORIZON_MODE)) {
 
@@ -172,6 +176,16 @@ 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 (CALC_OFFSET(errorGyroIf[axis]) < CALC_OFFSET(previousErrorGyroIf[axis])) {
+                previousErrorGyroIf[axis] = errorGyroIf[axis];
+            } else {
+                errorGyroIf[axis] = constrain(errorGyroIf[axis], -CALC_OFFSET(previousErrorGyroIf[axis]), CALC_OFFSET(previousErrorGyroIf[axis]));
+            }
+        } else {
+            previousErrorGyroIf[axis] = errorGyroIf[axis];
+        }
+
         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
         // I coefficient (I8) moved before integration to make limiting independent from PID settings
         ITerm = errorGyroIf[axis];
@@ -218,6 +232,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
     int32_t delta;
     int32_t PTerm, ITerm, DTerm;
     static int32_t lastError[3] = { 0, 0, 0 };
+    static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
     int32_t AngleRateTmp, RateError;
 
     int8_t horizonLevelStrength = 100;
@@ -299,8 +314,19 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
         // 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);
+
         ITerm = errorGyroI[axis] >> 13;
 
+        if (allowITermShrinkOnly) {
+            if (CALC_OFFSET(errorGyroI[axis]) < CALC_OFFSET(previousErrorGyroI[axis])) {
+                previousErrorGyroI[axis] = errorGyroI[axis];
+            } else {
+                errorGyroIf[axis] = constrain(errorGyroIf[axis], -CALC_OFFSET(previousErrorGyroI[axis]), CALC_OFFSET(previousErrorGyroI[axis]));
+            }
+        } else {
+            previousErrorGyroI[axis] = errorGyroI[axis];
+        }
+
         //-----calculate D-term
         delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
         lastError[axis] = RateError;

src/main/mw.c

@@ -103,7 +103,7 @@ enum {
 
 // AIR MODE Reset timers
 #define ERROR_RESET_DEACTIVATE_DELAY (1 * 1000)   // 1 sec delay to disable AIR MODE Iterm resetting
-#define ERROR_RESET_ACTIVATE_DELAY (5 * 1000)     // 5 sec delay to enable AIR MODE Iterm resetting
+bool allowITermShrinkOnly = false;
 
 uint32_t currentTime = 0;
 uint32_t previousTime = 0;
@@ -570,31 +570,28 @@ void processRx(void)
     throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 
     static bool airModeErrorResetIsEnabled = true; // Should always initialize with reset enabled
-    static uint32_t airModeErrorResetToggleTimeout = 0;  // Timeout for both activate and deactivate mode
+    static uint32_t airModeErrorResetTimeout = 0;  // Timeout for both activate and deactivate mode
 
     if (throttleStatus == THROTTLE_LOW)  {
-
+        // When in AIR Mode LOW Throttle and reset was already disabled we will only prevent further growing
-        // Evaluate if the error needs resetting again. Should be entered when in air mode during landings etc...
         if ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !airModeErrorResetIsEnabled) {
-            if ((millis() > airModeErrorResetToggleTimeout) && isRollPitchCentered()) {
+            if (isRollPitchCentered()) {
-                airModeErrorResetIsEnabled = true;
+                allowITermShrinkOnly = true;   // Iterm is now only allowed to shrink
             }
         }
 
         // Conditions to reset Error
         if (!IS_RC_MODE_ACTIVE(BOXARM) || feature(FEATURE_MOTOR_STOP) || ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && airModeErrorResetIsEnabled)) {
             pidResetErrorGyro();
-            airModeErrorResetToggleTimeout = millis() + ERROR_RESET_DEACTIVATE_DELAY; // Reset de-activate timer
+            airModeErrorResetTimeout = millis() + ERROR_RESET_DEACTIVATE_DELAY; // Reset de-activate timer
         }
     } else {
         if (!(feature(FEATURE_MOTOR_STOP)) && IS_RC_MODE_ACTIVE(BOXARM) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
             if (airModeErrorResetIsEnabled) {
-                if ((millis() > airModeErrorResetToggleTimeout) && !isRollPitchCentered()) {  // Only disable error reset when roll and pitch not centered
+                if ((millis() > airModeErrorResetTimeout) && !isRollPitchCentered()) {  // Only disable error reset when roll and pitch not centered
                     airModeErrorResetIsEnabled = false;
-                    airModeErrorResetToggleTimeout = millis() + ERROR_RESET_ACTIVATE_DELAY; // Start reset activate timer
+                    allowITermShrinkOnly = false;   // Reset shrinking for Iterm
                 }
-            } else if (!airModeErrorResetIsEnabled) {
-                airModeErrorResetToggleTimeout = millis() + ERROR_RESET_ACTIVATE_DELAY; // Reset activate timer
             }
         } else {
             airModeErrorResetIsEnabled = true;  // This is needed to reset procedure when switch is toggled in air