Merge pull request #4623 from iNavFlight/agh_nav_extra_arming_safety_bypass

Add support for bypassing extra arming safety

Author: fiam

docs/Cli.md

@@ -177,7 +177,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  name  | Empty string | Craft name |
 |  nav_disarm_on_landing  | OFF | If set to ON, iNav disarms the FC after landing |
 |  nav_use_midthr_for_althold  | OFF | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude |
-|  nav_extra_arming_safety  | ON | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured |
+|  nav_extra_arming_safety  | ON | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used |
 |  nav_user_control_mode  | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - right stick controls attitude like in ANGLE mode; CRUISE - right stick controls velocity in forward and right direction. |
 |  nav_position_timeout  | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
 |  nav_wp_radius  | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius |

src/main/fc/fc_core.c

@@ -215,7 +215,7 @@ static void updateArmingStatus(void)
 
 #if defined(USE_NAV)
         /* CHECK: Navigation safety */
-        if (navigationBlockArming()) {
+        if (navigationIsBlockingArming(NULL) != NAV_ARMING_BLOCKER_NONE) {
             ENABLE_ARMING_FLAG(ARMING_DISABLED_NAVIGATION_UNSAFE);
         }
         else {
@@ -374,6 +374,18 @@ void tryArm(void)
             return;
         }
 
+#if defined(USE_NAV)
+        // If nav_extra_arming_safety was bypassed we always
+        // allow bypassing it even without the sticks set
+        // in the correct position to allow re-arming quickly
+        // in case of a mid-air accidental disarm.
+        bool usedBypass = false;
+        navigationIsBlockingArming(&usedBypass);
+        if (usedBypass) {
+            ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED);
+        }
+#endif
+
         lastDisarmReason = DISARM_NONE;
 
         ENABLE_ARMING_FLAG(ARMED);

src/main/fc/runtime_config.h

@@ -89,20 +89,21 @@ extern uint32_t flightModeFlags;
 #define FLIGHT_MODE(mask) (flightModeFlags & (mask))
 
 typedef enum {
-    GPS_FIX_HOME                = (1 << 0),
-    GPS_FIX                     = (1 << 1),
-    CALIBRATE_MAG               = (1 << 2),
-    SMALL_ANGLE                 = (1 << 3),
-    FIXED_WING                  = (1 << 4),     // set when in flying_wing or airplane mode. currently used by althold selection code
-    ANTI_WINDUP                 = (1 << 5),
-    FLAPERON_AVAILABLE          = (1 << 6),
-    NAV_MOTOR_STOP_OR_IDLE      = (1 << 7),     // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available
-    COMPASS_CALIBRATED          = (1 << 8),
-    ACCELEROMETER_CALIBRATED    = (1 << 9),
-    PWM_DRIVER_AVAILABLE        = (1 << 10),
-    NAV_CRUISE_BRAKING          = (1 << 11),
-    NAV_CRUISE_BRAKING_BOOST    = (1 << 12),
-    NAV_CRUISE_BRAKING_LOCKED   = (1 << 13),
+    GPS_FIX_HOME                        = (1 << 0),
+    GPS_FIX                             = (1 << 1),
+    CALIBRATE_MAG                       = (1 << 2),
+    SMALL_ANGLE                         = (1 << 3),
+    FIXED_WING                          = (1 << 4),     // set when in flying_wing or airplane mode. currently used by althold selection code
+    ANTI_WINDUP                         = (1 << 5),
+    FLAPERON_AVAILABLE                  = (1 << 6),
+    NAV_MOTOR_STOP_OR_IDLE              = (1 << 7),     // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available
+    COMPASS_CALIBRATED                  = (1 << 8),
+    ACCELEROMETER_CALIBRATED            = (1 << 9),
+    PWM_DRIVER_AVAILABLE                = (1 << 10),
+    NAV_CRUISE_BRAKING                  = (1 << 11),
+    NAV_CRUISE_BRAKING_BOOST            = (1 << 12),
+    NAV_CRUISE_BRAKING_LOCKED           = (1 << 13),
+    NAV_EXTRA_ARMING_SAFETY_BYPASSED    = (1 << 14),    // nav_extra_arming_safey was bypassed. Keep it until power cycle.
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

src/main/fc/settings.yaml

@@ -120,6 +120,9 @@ tables:
   - name: iterm_relax_type
     values: ["GYRO", "SETPOINT"]
     enum: itermRelaxType_e
+  - name: nav_extra_arming_safety
+    values: ["OFF", "ON", "ALLOW_BYPASS"]
+    enum: navExtraArmingSafety_e
 
 groups:
   - name: PG_GYRO_CONFIG
@@ -1244,7 +1247,7 @@ groups:
         type: bool
       - name: nav_extra_arming_safety
         field: general.flags.extra_arming_safety
-        type: bool
+        table: nav_extra_arming_safety
       - name: nav_user_control_mode
         field: general.flags.user_control_mode
         table: nav_user_control_mode

src/main/io/osd.c

@@ -616,7 +616,20 @@ static const char * osdArmingDisabledReasonMessage(void)
         case ARMING_DISABLED_SYSTEM_OVERLOADED:
             return OSD_MESSAGE_STR("SYSTEM OVERLOADED");
         case ARMING_DISABLED_NAVIGATION_UNSAFE:
-            return OSD_MESSAGE_STR("NAVIGATION IS UNSAFE");
+#if defined(USE_NAV)
+            // Check the exact reason
+            switch (navigationIsBlockingArming(NULL)) {
+                case NAV_ARMING_BLOCKER_NONE:
+                    break;
+                case NAV_ARMING_BLOCKER_MISSING_GPS_FIX:
+                    return OSD_MESSAGE_STR("WAITING FOR GPS FIX");
+                case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE:
+                    return OSD_MESSAGE_STR("DISABLE NAVIGATION FIRST");
+                case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR:
+                    return OSD_MESSAGE_STR("FIRST WAYPOINT IS TOO FAR");
+            }
+#endif
+            break;
         case ARMING_DISABLED_COMPASS_NOT_CALIBRATED:
             return OSD_MESSAGE_STR("COMPASS NOT CALIBRATED");
         case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED:

src/main/navigation/navigation.c

@@ -50,6 +50,8 @@
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
+#include "rx/rx.h"
+
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
 #include "sensors/boardalignment.h"
@@ -83,7 +85,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
 
         .flags = {
             .use_thr_mid_for_althold = 0,
-            .extra_arming_safety = 1,
+            .extra_arming_safety = NAV_EXTRA_ARMING_SAFETY_ON,
             .user_control_mode = NAV_GPS_ATTI,
             .rth_alt_control_mode = NAV_RTH_AT_LEAST_ALT,
             .rth_climb_first = 1,                   // Climb first, turn after reaching safe altitude
@@ -2919,23 +2921,34 @@ bool navigationTerrainFollowingEnabled(void)
     return posControl.flags.isTerrainFollowEnabled;
 }
 
-bool navigationBlockArming(void)
+navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
 {
     const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
     const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
-    bool shouldBlockArming = false;
 
-    if (!navConfig()->general.flags.extra_arming_safety)
-        return false;
+    if (usedBypass) {
+        *usedBypass = false;
+    }
+
+    if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_OFF) {
+        return NAV_ARMING_BLOCKER_NONE;
+    }
 
     // Apply extra arming safety only if pilot has any of GPS modes configured
     if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
-        shouldBlockArming = true;
+        if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS &&
+            (STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || rxGetChannelValue(YAW) > 1750)) {
+            if (usedBypass) {
+                *usedBypass = true;
+            }
+            return NAV_ARMING_BLOCKER_NONE;
+        }
+        return NAV_ARMING_BLOCKER_MISSING_GPS_FIX;
     }
 
     // Don't allow arming if any of NAV modes is active
     if (!ARMING_FLAG(ARMED) && navBoxModesEnabled && !navLaunchComboModesEnabled) {
-        shouldBlockArming = true;
+        return NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE;
     }
 
     // Don't allow arming if first waypoint is farther than configured safe distance
@@ -2946,14 +2959,13 @@ bool navigationBlockArming(void)
         const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
 
         if (navWpMissionStartTooFar) {
-            shouldBlockArming = true;
+            return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
         }
     }
 
-    return shouldBlockArming;
+    return NAV_ARMING_BLOCKER_NONE;
 }
 
-
 bool navigationPositionEstimateIsHealthy(void)
 {
     return (posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME);

src/main/navigation/navigation.h

@@ -88,6 +88,19 @@ typedef enum {
     NAV_RTH_ALLOW_LANDING_FS_ONLY = 2, // Allow landing only if RTH was triggered by failsafe
 } navRTHAllowLanding_e;
 
+typedef enum {
+    NAV_EXTRA_ARMING_SAFETY_OFF = 0,
+    NAV_EXTRA_ARMING_SAFETY_ON = 1,
+    NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS = 2, // Allow disabling by holding THR+YAW low
+} navExtraArmingSafety_e;
+
+typedef enum {
+    NAV_ARMING_BLOCKER_NONE = 0,
+    NAV_ARMING_BLOCKER_MISSING_GPS_FIX = 1,
+    NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE = 2,
+    NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR = 3,
+} navArmingBlocker_e;
+
 typedef struct positionEstimationConfig_s {
     uint8_t automatic_mag_declination;
     uint8_t reset_altitude_type; // from nav_reset_type_e
@@ -128,7 +141,7 @@ typedef struct navConfig_s {
     struct {
         struct {
             uint8_t use_thr_mid_for_althold;    // Don't remember throttle when althold was initiated, assume that throttle is at Thr Mid = zero climb rate
-            uint8_t extra_arming_safety;        // Forcibly apply 100% throttle tilt compensation
+            uint8_t extra_arming_safety;        // from navExtraArmingSafety_e
             uint8_t user_control_mode;          // NAV_GPS_ATTI or NAV_GPS_CRUISE
             uint8_t rth_alt_control_mode;       // Controls the logic for choosing the RTH altitude
             uint8_t rth_climb_first;            // Controls the logic for initial RTH climbout
@@ -352,7 +365,10 @@ bool navigationRequiresAngleMode(void);
 bool navigationRequiresThrottleTiltCompensation(void);
 bool navigationRequiresTurnAssistance(void);
 int8_t navigationGetHeadingControlState(void);
-bool navigationBlockArming(void);
+// Returns wether arming is blocked by the navigation system.
+// If usedBypass is provided, it will indicate wether any checks
+// were bypassed due to user input.
+navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass);
 bool navigationPositionEstimateIsHealthy(void);
 bool navIsCalibrationComplete(void);
 bool navigationTerrainFollowingEnabled(void);