Rework Iterm Shrink for AIR Mode

Rework Iterm Shrink for AIR Mode part 2

src/main/flight/pid.c

@@ -321,7 +321,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
             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]));
+                errorGyroI[axis] = constrain(errorGyroI[axis], -CALC_OFFSET(previousErrorGyroI[axis]), CALC_OFFSET(previousErrorGyroI[axis]));
             }
         } else {
             previousErrorGyroI[axis] = errorGyroI[axis];

src/main/io/rc_controls.c

@@ -59,13 +59,11 @@
 
 #include "mw.h"
 
-
 static escAndServoConfig_t *escAndServoConfig;
 static pidProfile_t *pidProfile;
 
 // true if arming is done via the sticks (as opposed to a switch)
 static bool isUsingSticksToArm = true;
-static bool rollPitchCentered = true;      // Roll and pitch are centered, AIR Mode condition
 
 int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
 
@@ -107,11 +105,6 @@ bool isUsingSticksForArming(void)
     return isUsingSticksToArm;
 }
 
-bool isRollPitchCentered(void)
-{
-    return rollPitchCentered;
-}
-
 bool areSticksInApModePosition(uint16_t ap_mode)
 {
     return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
@@ -127,6 +120,18 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband
     return THROTTLE_HIGH;
 }
 
+rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig)
+{
+    if (feature(FEATURE_3D)) // TODO
+        return CENTERED;
+    else if (!feature(FEATURE_3D)
+            && ((rcData[PITCH] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[PITCH] > (rxConfig->midrc -AIRMODEDEADBAND)))
+            && ((rcData[ROLL] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[ROLL] > (rxConfig->midrc -AIRMODEDEADBAND))))
+        return CENTERED;
+
+    return NOT_CENTERED;
+}
+
 void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch)
 {
     static uint8_t rcDelayCommand;      // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
@@ -151,12 +156,6 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
         rcDelayCommand = 0;
     rcSticks = stTmp;
 
-    if (rcSticks == PIT_CE + ROL_CE) {
-        rollPitchCentered = true;
-    } else {
-        rollPitchCentered = false;
-    }
-
     // perform actions
     if (!isUsingSticksToArm) {
 

src/main/io/rc_controls.h

@@ -77,6 +77,13 @@ typedef enum {
     THROTTLE_HIGH
 } throttleStatus_e;
 
+#define AIRMODEDEADBAND 10
+
+typedef enum {
+    NOT_CENTERED = 0,
+    CENTERED
+} rollPitchStatus_e;
+
 #define ROL_LO (1 << (2 * ROLL))
 #define ROL_CE (3 << (2 * ROLL))
 #define ROL_HI (2 << (2 * ROLL))
@@ -243,4 +250,4 @@ bool isUsingSticksForArming(void);
 
 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
 bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
-bool isRollPitchCentered(void);
+rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig);

src/main/mw.c

@@ -20,7 +20,6 @@
 #include <stdint.h>
 #include <math.h>
 
-#include "debug.h"
 #include "platform.h"
 
 #include "common/maths.h"
@@ -575,26 +574,30 @@ void processRx(void)
     if (throttleStatus == THROTTLE_LOW)  {
         // When in AIR Mode LOW Throttle and reset was already disabled we will only prevent further growing
         if ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !airModeErrorResetIsEnabled)  {
-            if (isRollPitchCentered()) {
+            if (calculateRollPitchCenterStatus(&masterConfig.rxConfig) == CENTERED) {
                 allowITermShrinkOnly = true;   // Iterm is now only allowed to shrink
+            } else {
+                allowITermShrinkOnly = false;   // Iterm should considered safe to increase
             }
         }
 
         // Conditions to reset Error
-        if (!IS_RC_MODE_ACTIVE(BOXARM) || feature(FEATURE_MOTOR_STOP) || ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && airModeErrorResetIsEnabled)) {
+        if (!ARMING_FLAG(ARMED) || feature(FEATURE_MOTOR_STOP) || ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && airModeErrorResetIsEnabled) || !IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
             pidResetErrorGyro();
             airModeErrorResetTimeout = millis() + ERROR_RESET_DEACTIVATE_DELAY; // Reset de-activate timer
+            airModeErrorResetIsEnabled = true;                                  // Enable Reset again especially after Disarm
+            allowITermShrinkOnly = false;                                       // Disable shrink especially after Disarm
         }
     } else {
-        if (!(feature(FEATURE_MOTOR_STOP)) && IS_RC_MODE_ACTIVE(BOXARM) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
+        if (!(feature(FEATURE_MOTOR_STOP)) && ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
             if (airModeErrorResetIsEnabled) {
-                if ((millis() > airModeErrorResetTimeout) && !isRollPitchCentered()) {  // Only disable error reset when roll and pitch not centered
+                if (millis() > airModeErrorResetTimeout && calculateRollPitchCenterStatus(&masterConfig.rxConfig) == NOT_CENTERED) {  // Only disable error reset when roll and pitch not centered
                     airModeErrorResetIsEnabled = false;
                     allowITermShrinkOnly = false;   // Reset shrinking for Iterm
                 }
-            }
             } else {
-            airModeErrorResetIsEnabled = true;  // This is needed to reset procedure when switch is toggled in air
+                allowITermShrinkOnly = false;   // Reset shrinking for Iterm
+            }
         }
     }