In flight emergency rearm

src/main/fc/fc_core.c

@@ -106,6 +106,7 @@ enum {
 #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000
 #define EMERGENCY_ARMING_COUNTER_STEP_MS 1000
 #define EMERGENCY_ARMING_MIN_ARM_COUNT 10
+#define EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS 5000
 
 timeDelta_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
 static timeUs_t flightTime = 0;
@@ -120,6 +121,7 @@ uint8_t motorControlEnable = false;
 static bool isRXDataNew;
 static disarmReason_t lastDisarmReason = DISARM_NONE;
 timeUs_t lastDisarmTimeUs = 0;
+timeMs_t emergRearmStabiliseTimeout = 0;
 
 static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible
 static timeMs_t prearmActivationTime = 0;
@@ -315,11 +317,11 @@ static void updateArmingStatus(void)
 
         /* CHECK: Do not allow arming if Servo AutoTrim is enabled */
         if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
-	       ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
-	    }
+           ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
+        }
         else {
-	       DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
-	    }
+           DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
+        }
 
 #ifdef USE_DSHOT
         /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */
@@ -435,6 +437,7 @@ void disarm(disarmReason_t disarmReason)
         lastDisarmReason = disarmReason;
         lastDisarmTimeUs = micros();
         DISABLE_ARMING_FLAG(ARMED);
+        DISABLE_STATE(IN_FLIGHT_EMERG_REARM);
 
 #ifdef USE_BLACKBOX
         if (feature(FEATURE_BLACKBOX)) {
@@ -505,14 +508,27 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm)
     return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT;
 }
 
-#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
+bool emergInflightRearmEnabled(void)
+{
+    /* Emergency rearm allowed within 5s timeout period after disarm if craft still flying */
+    timeMs_t currentTimeMs = millis();
+    emergRearmStabiliseTimeout = 0;
 
-void releaseSharedTelemetryPorts(void) {
-    serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
-    while (sharedPort) {
-        mspSerialReleasePortIfAllocated(sharedPort);
-        sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
+    if ((lastDisarmReason != DISARM_SWITCH && lastDisarmReason != DISARM_KILLSWITCH) ||
+        (currentTimeMs > US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS)) {
+        return false;
     }
+
+    // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying
+    bool mcDisarmVertVelCheck = STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f;
+
+    if (isProbablyStillFlying() || mcDisarmVertVelCheck) {
+        emergRearmStabiliseTimeout = currentTimeMs + 5000;  // activate Angle mode for 5s after rearm to help stabilise craft
+        ENABLE_STATE(IN_FLIGHT_EMERG_REARM);
+        return true;
+    }
+
+    return false;   // craft doesn't appear to be flying, don't allow emergency rearm
 }
 
 void tryArm(void)
@@ -538,9 +554,10 @@ void tryArm(void)
 #endif
 
 #ifdef USE_PROGRAMMING_FRAMEWORK
-    if (!isArmingDisabled() || emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) {
+    if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled() ||
+        LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) {
 #else
-    if (!isArmingDisabled() || emergencyArmingIsEnabled()) {
+    if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled()) {
 #endif
         // If nav_extra_arming_safety was bypassed we always
         // allow bypassing it even without the sticks set
@@ -558,11 +575,14 @@ void tryArm(void)
         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
         //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
         ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
-        logicConditionReset();
 
+        if (!STATE(IN_FLIGHT_EMERG_REARM)) {
+            resetLandingDetectorActiveState();  // reset landing detector after arming to avoid false detection before flight
+            logicConditionReset();
 #ifdef USE_PROGRAMMING_FRAMEWORK
-        programmingPidReset();
+            programmingPidReset();
 #endif
+        }
 
         headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 
@@ -595,6 +615,16 @@ void tryArm(void)
     }
 }
 
+#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
+
+void releaseSharedTelemetryPorts(void) {
+    serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
+    while (sharedPort) {
+        mspSerialReleasePortIfAllocated(sharedPort);
+        sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
+    }
+}
+
 void processRx(timeUs_t currentTimeUs)
 {
     // Calculate RPY channel data
@@ -645,9 +675,12 @@ void processRx(timeUs_t currentTimeUs)
         processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData);
     }
 
+    // Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR)
+    bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs);
+    bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce;
     bool canUseHorizonMode = true;
 
-    if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
+    if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) {
         // bumpless transfer to Level mode
         canUseHorizonMode = false;
 
@@ -816,7 +849,6 @@ void processRx(timeUs_t currentTimeUs)
         }
     }
 #endif
-
 }
 
 // Function for loop trigger
@@ -876,6 +908,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
         processDelayedSave();
     }
 
+    if (armTime > 1 * USECS_PER_SEC) {     // reset in flight emerg rearm flag 1 sec after arming once it's served its purpose
+        DISABLE_STATE(IN_FLIGHT_EMERG_REARM);
+    }
+
 #if defined(SITL_BUILD)
     if (lockMainPID()) {
 #endif
@@ -925,15 +961,15 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     //Servos should be filtered or written only when mixer is using servos or special feaures are enabled
 
 #ifdef USE_SIMULATOR
-	if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) {
-	    if (isServoOutputEnabled()) {
-	        writeServos();
-	    }
-
-	    if (motorControlEnable) {
-	        writeMotors();
-	    }
-	}
+    if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) {
+        if (isServoOutputEnabled()) {
+            writeServos();
+        }
+
+        if (motorControlEnable) {
+            writeMotors();
+        }
+    }
 #else
     if (isServoOutputEnabled()) {
         writeServos();

src/main/fc/runtime_config.h

@@ -139,6 +139,7 @@ typedef enum {
     FW_HEADING_USE_YAW                  = (1 << 24),
     ANTI_WINDUP_DEACTIVATED             = (1 << 25),
     LANDING_DETECTED                    = (1 << 26),
+    IN_FLIGHT_EMERG_REARM               = (1 << 27),
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

src/main/io/osd.c

@@ -1201,7 +1201,7 @@ int32_t osdGetAltitude(void)
 
 static inline int32_t osdGetAltitudeMsl(void)
 {
-    return getEstimatedActualPosition(Z)+GPS_home.alt;
+    return getEstimatedActualPosition(Z) + posControl.gpsOrigin.alt;
 }
 
 uint16_t osdGetRemainingGlideTime(void) {
@@ -1911,7 +1911,7 @@ static bool osdDrawSingleElement(uint8_t item)
         osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0);
         break;
 
-    case OSD_ODOMETER:    
+    case OSD_ODOMETER:
         {
             displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_ODOMETER);
             uint32_t odometerDist = (uint32_t)(getTotalTravelDistance() / 100);
@@ -3955,7 +3955,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
 
 /**
  * @brief Draws the INAV and/or pilot logos on the display
- * 
+ *
  * @param singular If true, only one logo will be drawn. If false, both logos will be drawn, separated by osdConfig()->inav_to_pilot_logo_spacing characters
  * @param row The row number to start drawing the logos. If not singular, both logos are drawn on the same rows.
  * @return uint8_t The row number after the logo(s).
@@ -4005,7 +4005,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
         } else {
             logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing;
         }
-        
+
         for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) {
             for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) {
                 displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++);
@@ -4129,7 +4129,7 @@ static void osdCompleteAsyncInitialization(void)
 
     if (fontHasMetadata && metadata.charCount > 256) {
         hasExtendedFont = true;
-    
+
         y = drawLogos(false, y);
         y++;
     } else if (!fontHasMetadata) {
@@ -4568,12 +4568,12 @@ static void osdShowHDArmScreen(void)
 
                 displayWrite(osdDisplayPort, col, armScreenRow, buf);
                 displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2);
-                
+
                 int digits = osdConfig()->plus_code_digits;
                 olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
                 displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
             }
-            
+
 #if defined (USE_SAFE_HOME)
             if (posControl.safehomeState.distance) { // safehome found during arming
                 if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
@@ -4653,7 +4653,7 @@ static void osdShowSDArmScreen(void)
                 uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2;
                 displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf);
                 displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2);
-                
+
                 int digits = osdConfig()->plus_code_digits;
                 olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
                 displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
@@ -4806,10 +4806,14 @@ static void osdRefresh(timeUs_t currentTimeUs)
             statsDisplayed = false;
             osdResetStats();
             osdShowArmed();
-            uint32_t delay = osdConfig()->arm_screen_display_time;
+            uint16_t delay = osdConfig()->arm_screen_display_time;
+            if (STATE(IN_FLIGHT_EMERG_REARM)) {
+                delay = 500;
+            }
 #if defined(USE_SAFE_HOME)
-            if (posControl.safehomeState.distance)
-                delay+= 3000;
+            else if (posControl.safehomeState.distance) {
+                delay += 3000;
+            }
 #endif
             osdSetNextRefreshIn(delay);
         } else {