AIR MODE Safety Enhancements

src/main/flight/pid.c

@@ -61,8 +61,6 @@ uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
 
 static int32_t errorGyroI[3] = { 0, 0, 0 };
 static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
-static int32_t errorAngleI[2] = { 0, 0 };
-static float errorAngleIf[2] = { 0.0f, 0.0f };
 
 static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
@@ -72,15 +70,6 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
 
 pidControllerFuncPtr pid_controller = pidRewrite; // which pid controller are we using, defaultMultiWii
 
-void pidResetErrorAngle(void)
-{
-    errorAngleI[ROLL] = 0;
-    errorAngleI[PITCH] = 0;
-
-    errorAngleIf[ROLL] = 0.0f;
-    errorAngleIf[PITCH] = 0.0f;
-}
-
 void pidResetErrorGyro(void)
 {
     errorGyroI[ROLL] = 0;

src/main/flight/pid.h

@@ -75,6 +75,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 pidResetErrorAngle(void);
 void pidResetErrorGyro(void);
 

src/main/io/rc_controls.c

@@ -65,6 +65,7 @@ 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
 
@@ -106,6 +107,10 @@ bool isUsingSticksForArming(void)
     return isUsingSticksToArm;
 }
 
+bool isRollPitchCentered(void)
+{
+    return rollPitchCentered;
+}
 
 bool areSticksInApModePosition(uint16_t ap_mode)
 {
@@ -146,6 +151,12 @@ 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

@@ -126,7 +126,6 @@ typedef struct modeActivationCondition_s {
 
 #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
 
-#define SHOULD_RESET_ERRORS ((throttleStatus == THROTTLE_LOW && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) || !(ARMING_FLAG(ARMED)) || ((throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))))
 typedef struct controlRateConfig_s {
     uint8_t rcRate8;
     uint8_t rcExpo8;
@@ -244,3 +243,4 @@ bool isUsingSticksForArming(void);
 
 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
 bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
+bool isRollPitchCentered(void);

src/main/mw.c

@@ -101,6 +101,10 @@ enum {
 #define PREVENT_BARO_READ_PRE_LOOP_TRIGGER 150 // Prevent BARO processing before expected loop trigger
 #define GYRO_RATE 0.001f  // Gyro refresh rate 1khz
 
+// 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
+
 uint32_t currentTime = 0;
 uint32_t previousTime = 0;
 uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
@@ -565,9 +569,36 @@ void processRx(void)
 
     throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 
-    if (SHOULD_RESET_ERRORS)  {
-        pidResetErrorAngle();
+    static bool airModeErrorResetIsEnabled = true;       // Should always initialize with reset enabled
+    static uint32_t airModeErrorResetToggleTimeout = 0;  // Timeout for both activate and deactivate mode
+
+    if (throttleStatus == THROTTLE_LOW)  {
+
+        // 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()) {
+                airModeErrorResetIsEnabled = true;
+            }
+        }
+
+        // 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
+        }
+    } 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
+                    airModeErrorResetIsEnabled = false;
+                    airModeErrorResetToggleTimeout = millis() + ERROR_RESET_ACTIVATE_DELAY; // Start reset activate timer
+                }
+            } 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
+        }
     }
 
     // When armed and motors aren't spinning, do beeps and then disarm
@@ -634,7 +665,6 @@ void processRx(void)
     	canUseHorizonMode = false;
 
         if (!FLIGHT_MODE(ANGLE_MODE)) {
-            pidResetErrorAngle();
             ENABLE_FLIGHT_MODE(ANGLE_MODE);
         }
     } else {
@@ -646,7 +676,6 @@ void processRx(void)
         DISABLE_FLIGHT_MODE(ANGLE_MODE);
 
         if (!FLIGHT_MODE(HORIZON_MODE)) {
-            pidResetErrorAngle();
             ENABLE_FLIGHT_MODE(HORIZON_MODE);
         }
     } else {