Change logic + add gyro check for MR

src/main/fc/fc_core.c

@@ -119,8 +119,7 @@ uint8_t motorControlEnable = false;
 static bool isRXDataNew;
 static disarmReason_t lastDisarmReason = DISARM_NONE;
 timeUs_t lastDisarmTimeUs = 0;
-timeMs_t emergInflightRearmTimeout = 0;
+timeMs_t emergRearmStabiliseTimeout = 0;
-timeMs_t mcEmergRearmStabiliseTimeout = 0;
 
 static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible
 static timeMs_t prearmActivationTime = 0;
@@ -435,10 +434,6 @@ void disarm(disarmReason_t disarmReason)
     if (ARMING_FLAG(ARMED)) {
         lastDisarmReason = disarmReason;
         lastDisarmTimeUs = micros();
-        if (disarmReason == DISARM_SWITCH || disarmReason == DISARM_KILLSWITCH) {
-            emergInflightRearmTimeout = isProbablyStillFlying() ? US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0;
-        }
-
         DISABLE_ARMING_FLAG(ARMED);
 
 #ifdef USE_BLACKBOX
@@ -508,24 +503,24 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn)
 
 bool emergInflightRearmEnabled(void)
 {
-    /* Emergency rearm allowed within emergInflightRearmTimeout window.
+    /* Emergency rearm allowed within 5s timeout period after disarm if craft still flying */
-     * On MR emergency rearm only allowed after 1.5s if MR dropping or climbing after disarm, i.e. still in flight */
-
     timeMs_t currentTimeMs = millis();
-    if (!emergInflightRearmTimeout || currentTimeMs > emergInflightRearmTimeout) {
+    emergRearmStabiliseTimeout = 0;
+
+    if ((lastDisarmReason != DISARM_SWITCH && lastDisarmReason != DISARM_KILLSWITCH) ||
+        (currentTimeMs > US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS)) {
         return false;
     }
 
-    if (STATE(MULTIROTOR)) {
+    if (isProbablyStillFlying()) {
-        uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500;  // check MR vertical speed at least 1.5 sec after disarm
+        emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft
-        if (fabsf(getEstimatedActualVelocity(Z)) < 100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) {
+        return true;
-            return false;       // MR doesn't appear to be flying so don't allow emergency rearm
+    } else if (STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f) {
-        } else {
+        // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying
-            mcEmergRearmStabiliseTimeout = currentTimeMs + 5000;    // activate Angle mode for 5s after rearm to help stabilise MR
+        return true;
-        }
     }
 
-    return true;
+    return false;   // craft doesn't appear to be flying, don't allow emergency rearm
 }
 
 void tryArm(void)
@@ -564,7 +559,6 @@ void tryArm(void)
         }
 
         lastDisarmReason = DISARM_NONE;
-        emergInflightRearmTimeout = 0;
 
         ENABLE_ARMING_FLAG(ARMED);
         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
@@ -667,9 +661,9 @@ void processRx(timeUs_t currentTimeUs)
         processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData);
     }
 
-    // Angle mode forced on briefly after multirotor emergency inflight rearm to help stabilise attitude
+    // Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR)
-    bool mcEmergRearmAngleEnforce = STATE(MULTIROTOR) && mcEmergRearmStabiliseTimeout > millis();
+    bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs);
-    bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mcEmergRearmAngleEnforce;
+    bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce;
     bool canUseHorizonMode = true;
 
     if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) {

src/main/navigation/navigation.c

@@ -61,6 +61,7 @@
 #include "sensors/acceleration.h"
 #include "sensors/boardalignment.h"
 #include "sensors/battery.h"
+#include "sensors/gyro.h"
 
 #include "programming/global_variables.h"
 
@@ -2808,13 +2809,16 @@ void updateLandingStatus(timeMs_t currentTimeMs)
     DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED));
 
     if (!ARMING_FLAG(ARMED)) {
-        resetLandingDetector();
+        if (STATE(LANDING_DETECTED)) {
-        landingDetectorIsActive = false;
+            resetLandingDetector();
-
+            landingDetectorIsActive = false;
+        }
         if (!IS_RC_MODE_ACTIVE(BOXARM)) {
             DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
         }
         return;
+    } else if (getArmTime() < 0.25f && landingDetectorIsActive) {  // force reset landing detector immediately after arming
+        landingDetectorIsActive = false;
     }
 
     if (!landingDetectorIsActive) {
@@ -2854,10 +2858,14 @@ bool isFlightDetected(void)
 
 bool isProbablyStillFlying(void)
 {
-    // Multirotor flight sanity checked after disarm so always true here
+    bool inFlightSanityCheck;
-    bool inFlightSanityCheck = STATE(MULTIROTOR) || (STATE(AIRPLANE) && isGPSHeadingValid());
+    if (STATE(MULTIROTOR)) {
+        inFlightSanityCheck = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING || averageAbsGyroRates() > 4.0f;
+    } else {
+        inFlightSanityCheck = isGPSHeadingValid();
+    }
 
-    return landingDetectorIsActive && !STATE(LANDING_DETECTED) && inFlightSanityCheck;
+    return landingDetectorIsActive && inFlightSanityCheck;
 }
 
 /*-----------------------------------------------------------