prevent unwanted resets on disarm

iNavFlight · Sep 26, 2023 · 52f5cff · 52f5cff
1 parent 45272d9
commit 52f5cff
Show file tree

Hide file tree

Showing 5 changed files with 67 additions and 26 deletions.
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
@@ -436,6 +436,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)) {
@@ -522,6 +523,7 @@ bool emergInflightRearmEnabled(void)
 
     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;
     }
 
@@ -574,11 +576,13 @@ 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)) {
+            logicConditionReset();
 #ifdef USE_PROGRAMMING_FRAMEWORK
-        programmingPidReset();
+            programmingPidReset();
 #endif
+        }
 
         headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 
@@ -902,6 +906,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

diff --git a/src/main/fc/runtime_config.h b/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))

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
@@ -4579,9 +4579,13 @@ static void osdRefresh(timeUs_t currentTimeUs)
             osdResetStats();
             osdShowArmed();
             uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
+            if (STATE(IN_FLIGHT_EMERG_REARM)) {
+                delay /= 3;
+            }
 #if defined(USE_SAFE_HOME)
-            if (posControl.safehomeState.distance)
+            else if (posControl.safehomeState.distance) {
                 delay *= 3;
+            }
 #endif
             osdSetNextRefreshIn(delay);
         } else {

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -2530,11 +2530,15 @@ bool findNearestSafeHome(void)
  *-----------------------------------------------------------*/
 void updateHomePosition(void)
 {
-    // Disarmed and have a valid position, constantly update home
+    // Disarmed and have a valid position, constantly update home before first arm (depending on setting)
+    // Update immediately after arming thereafter if reset on each arm (required to avoid home reset after emerg in flight rearm)
+    static bool setHome = false;
+    navSetWaypointFlags_t homeUpdateFlags = NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING;
+
     if (!ARMING_FLAG(ARMED)) {
         if (posControl.flags.estPosStatus >= EST_USABLE) {
             const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z;
-            bool setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags;
+            setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags;
             switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) {
                 case NAV_RESET_NEVER:
                     break;
@@ -2545,24 +2549,16 @@ void updateHomePosition(void)
                     setHome = true;
                     break;
             }
-            if (setHome) {
-#if defined(USE_SAFE_HOME)
-                findNearestSafeHome();
-#endif
-                setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
-                // save the current location in case it is replaced by a safehome or HOME_RESET
-                posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
-            }
         }
     }
     else {
         static bool isHomeResetAllowed = false;
-
         // If pilot so desires he may reset home position to current position
         if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
             if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
-                const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
-                setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
+                homeUpdateFlags = 0;
+                homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+                setHome = true;
                 isHomeResetAllowed = false;
             }
         }
@@ -2577,6 +2573,18 @@ void updateHomePosition(void)
             posControl.homeDirection = calculateBearingToDestination(tmpHomePos);
             updateHomePositionCompatibility();
         }
+
+        setHome &= !STATE(IN_FLIGHT_EMERG_REARM);   // prevent reset following emerg in flight rearm
+    }
+
+    if (setHome && (!ARMING_FLAG(WAS_EVER_ARMED) || ARMING_FLAG(ARMED))) {
+#if defined(USE_SAFE_HOME)
+        findNearestSafeHome();
+#endif
+        setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity());
+        // save the current location in case it is replaced by a safehome or HOME_RESET
+        posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
+        setHome = false;
     }
 }
 

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
@@ -108,15 +108,35 @@ static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t cur
     }
 }
 
-static bool shouldResetReferenceAltitude(void)
+static bool shouldResetReferenceAltitude(uint8_t updateSource)
 {
-    switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) {
-        case NAV_RESET_NEVER:
-            return false;
-        case NAV_RESET_ON_FIRST_ARM:
-            return !ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED);
-        case NAV_RESET_ON_EACH_ARM:
-            return !ARMING_FLAG(ARMED);
+    /* Altitude reference reset constantly before first arm.
+     * After first arm altitude ref only reset immediately after arming (to avoid reset after emerg in flight rearm)
+     * Need to check reset status for both altitude sources immediately after rearming before reset complete */
+
+    static bool resetAltitudeOnArm = false;
+    if (ARMING_FLAG(ARMED) && resetAltitudeOnArm) {
+        static uint8_t sourceCheck = 0;
+        sourceCheck |= updateSource;
+        bool allAltitudeSources = STATE(GPS_FIX) && sensors(SENSOR_BARO);
+
+        if ((allAltitudeSources && sourceCheck > SENSOR_GPS) || (!allAltitudeSources && sourceCheck)) {
+            resetAltitudeOnArm = false;
+            sourceCheck = 0;
+        }
+        return !STATE(IN_FLIGHT_EMERG_REARM);
+    }
+
+    if (!ARMING_FLAG(ARMED)) {
+        switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) {
+            case NAV_RESET_NEVER:
+                return false;
+            case NAV_RESET_ON_FIRST_ARM:
+                break;
+            case NAV_RESET_ON_EACH_ARM:
+                resetAltitudeOnArm = true;
+        }
+        return !ARMING_FLAG(WAS_EVER_ARMED);
     }
 
     return false;
@@ -226,7 +246,7 @@ void onNewGPSData(void)
         if (!posControl.gpsOrigin.valid) {
             geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_SET);
         }
-        else if (shouldResetReferenceAltitude()) {
+        else if (shouldResetReferenceAltitude(SENSOR_GPS)) {
             /* If we were never armed - keep altitude at zero */
             geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_RESET_ALTITUDE);
         }
@@ -309,7 +329,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
     float newBaroAlt = baroCalculateAltitude();
 
     /* If we are required - keep altitude at zero */
-    if (shouldResetReferenceAltitude()) {
+    if (shouldResetReferenceAltitude(SENSOR_BARO)) {
         initialBaroAltitudeOffset = newBaroAlt;
     }