From 6efc681168869d0c540ef25ae87a5ac527dd5789 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 4 Jun 2022 11:34:42 +0100
Subject: [PATCH 1/6] First build

---
 src/main/cms/cms_menu_navigation.c |  2 +-
 src/main/fc/settings.yaml          |  9 +++++----
 src/main/flight/failsafe.c         | 18 ++++++++++++++----
 src/main/flight/failsafe.h         |  2 +-
 4 files changed, 21 insertions(+), 10 deletions(-)

diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c
index 80f7bf09e1..64ff46989b 100644
--- a/src/main/cms/cms_menu_navigation.c
+++ b/src/main/cms/cms_menu_navigation.c
@@ -189,7 +189,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] =
     OSD_LABEL_ENTRY("-- MISSIONS --"),
 
     OSD_SETTING_ENTRY("MC WP SLOWDOWN", SETTING_NAV_MC_WP_SLOWDOWN),
-    OSD_SETTING_ENTRY("MISSION FAILSAFE", SETTING_FAILSAFE_MISSION),
+    OSD_SETTING_ENTRY("WP FAILSAFE DELAY", SETTING_FAILSAFE_MISSION_DELAY),
     OSD_SETTING_ENTRY("WP LOAD ON BOOT", SETTING_NAV_WP_LOAD_ON_BOOT),
     OSD_SETTING_ENTRY("WP REACHED RADIUS", SETTING_NAV_WP_RADIUS),
     OSD_SETTING_ENTRY("WP ENFORCE ALTITUDE", SETTING_NAV_WP_ENFORCE_ALTITUDE),
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 78e7e940f0..52579937ec 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -921,10 +921,11 @@ groups:
         description: "What failsafe procedure to initiate in Stage 2 when craft is closer to home than failsafe_min_distance. See [Failsafe documentation](Failsafe.md#failsafe_throttle)."
         default_value: "DROP"
         table: failsafe_procedure
-      - name: failsafe_mission
-        description: "If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode"
-        default_value: ON
-        type: bool
+      - name: failsafe_mission_delay
+        description: "Applies when a WP mission is in progress. Sets the time delay in seconds between failsafe activating and the defined Failsafe procedure triggering. If set to 0 the Failsafe procedure won't be triggered at all and the mission will continue until the end."
+        default_value: 1
+        min: 0
+        max: 600
 
   - name: PG_LIGHTS_CONFIG
     type: lightsConfig_t
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 0e761f3bd0..6cb1f15ae8 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -73,7 +73,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT,                                   // 0.5 sec
     .failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT,                 // 0.5 seconds (plus 200ms explicit delay)
     .failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT,                           // 20sec
-    .failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT,                                   // default throttle low delay for "just disarm" on failsafe condition
+    .failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT,         // default throttle low delay for "just disarm" on failsafe condition
     .failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT,                           // default full failsafe procedure
     .failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT,                   // 20 deg left
     .failsafe_fw_pitch_angle = SETTING_FAILSAFE_FW_PITCH_ANGLE_DEFAULT,                 // 10 deg dive (yes, positive means dive)
@@ -81,7 +81,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_stick_motion_threshold = SETTING_FAILSAFE_STICK_THRESHOLD_DEFAULT,
     .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT,                     // No minimum distance for failsafe by default
     .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure
-    .failsafe_mission = SETTING_FAILSAFE_MISSION_DEFAULT,                               // Enable failsafe in WP mode or not
+    .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // Time delay before Failsafe triggered during WP mission (s)
 );
 
 typedef enum {
@@ -336,8 +336,18 @@ static bool failsafeCheckStickMotion(void)
 
 static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
 {
-    if ((FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && !failsafeConfig()->failsafe_mission) {
-        return FAILSAFE_PROCEDURE_NONE;
+    static timeMs_t wpModeDelayedFailsafeStart = 0;
+    if ((FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
+        if (!wpModeDelayedFailsafeStart) {
+            wpModeDelayedFailsafeStart = millis();
+        } else {
+            if ((millis() - wpModeDelayedFailsafeStart <  (MILLIS_PER_SECOND * failsafeConfig()->failsafe_mission_delay)) ||
+                !failsafeConfig()->failsafe_mission_delay) {
+                return FAILSAFE_PROCEDURE_NONE;
+            } else {
+                wpModeDelayedFailsafeStart = 0;
+            }
+        }
     }
 
     // Craft is closer than minimum failsafe procedure distance (if set to non-zero)
diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h
index 0a118d733d..369962aaa2 100644
--- a/src/main/flight/failsafe.h
+++ b/src/main/flight/failsafe.h
@@ -42,7 +42,7 @@ typedef struct failsafeConfig_s {
     uint16_t failsafe_stick_motion_threshold;
     uint16_t failsafe_min_distance;             // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default)
     uint8_t failsafe_min_distance_procedure;    // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)
-    bool failsafe_mission;                      // Enable failsafe in WP mode or not
+    uint16_t failsafe_mission_delay;            // Time delay before Failsafe triggered when WP mission in progress (s)
 } failsafeConfig_t;
 
 PG_DECLARE(failsafeConfig_t, failsafeConfig);

From 9ffc0d91a4cfa3c3dbbd00c30323b22eb4252b18 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 4 Jun 2022 11:36:14 +0100
Subject: [PATCH 2/6] Update Settings.md

---
 docs/Settings.md | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 5d43cfe98b..72624e3a41 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -962,13 +962,13 @@ What failsafe procedure to initiate in Stage 2 when craft is closer to home than
 
 ---
 
-### failsafe_mission
+### failsafe_mission_delay
 
-If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode
+Applies when a WP mission is in progress. Sets the time delay in seconds between failsafe activating and the defined Failsafe procedure triggering. If set to 0 the Failsafe procedure won't be triggered at all and the mission will continue until the end.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| ON | OFF | ON |
+| 1 | 0 | 600 |
 
 ---
 

From 4a455c4613a2d1eb8827674666235ee05facde7a Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 6 Jun 2022 11:58:19 +0100
Subject: [PATCH 3/6] update settings

---
 docs/Settings.md           | 4 ++--
 src/main/fc/settings.yaml  | 6 +++---
 src/main/flight/failsafe.c | 4 ++--
 src/main/flight/failsafe.h | 2 +-
 4 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 72624e3a41..631e10f7c5 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -964,11 +964,11 @@ What failsafe procedure to initiate in Stage 2 when craft is closer to home than
 
 ### failsafe_mission_delay
 
-Applies when a WP mission is in progress. Sets the time delay in seconds between failsafe activating and the defined Failsafe procedure triggering. If set to 0 the Failsafe procedure won't be triggered at all and the mission will continue until the end.
+Applies when a WP mission is in progress. Sets the time delay in seconds between failsafe activating and the defined Failsafe procedure triggering. If set to -1 the Failsafe procedure won't be triggered at all and the mission will continue until the end.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 1 | 0 | 600 |
+| 0 | -1 | 600 |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 52579937ec..80cfca4986 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -922,9 +922,9 @@ groups:
         default_value: "DROP"
         table: failsafe_procedure
       - name: failsafe_mission_delay
-        description: "Applies when a WP mission is in progress. Sets the time delay in seconds between failsafe activating and the defined Failsafe procedure triggering. If set to 0 the Failsafe procedure won't be triggered at all and the mission will continue until the end."
-        default_value: 1
-        min: 0
+        description: "Applies when a WP mission is in progress. Sets the time delay in seconds between failsafe activating and the defined Failsafe procedure triggering. If set to -1 the Failsafe procedure won't be triggered at all and the mission will continue until the end."
+        default_value: 0
+        min: -1
         max: 600
 
   - name: PG_LIGHTS_CONFIG
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 6cb1f15ae8..2c666c1588 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -341,8 +341,8 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
         if (!wpModeDelayedFailsafeStart) {
             wpModeDelayedFailsafeStart = millis();
         } else {
-            if ((millis() - wpModeDelayedFailsafeStart <  (MILLIS_PER_SECOND * failsafeConfig()->failsafe_mission_delay)) ||
-                !failsafeConfig()->failsafe_mission_delay) {
+            if (failsafeConfig()->failsafe_mission_delay == -1 ||
+                (millis() - wpModeDelayedFailsafeStart <  (MILLIS_PER_SECOND * (uint16_t)failsafeConfig()->failsafe_mission_delay))) {
                 return FAILSAFE_PROCEDURE_NONE;
             } else {
                 wpModeDelayedFailsafeStart = 0;
diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h
index 369962aaa2..fcac289ad7 100644
--- a/src/main/flight/failsafe.h
+++ b/src/main/flight/failsafe.h
@@ -42,7 +42,7 @@ typedef struct failsafeConfig_s {
     uint16_t failsafe_stick_motion_threshold;
     uint16_t failsafe_min_distance;             // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default)
     uint8_t failsafe_min_distance_procedure;    // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)
-    uint16_t failsafe_mission_delay;            // Time delay before Failsafe triggered when WP mission in progress (s)
+    int16_t failsafe_mission_delay;             // Time delay before Failsafe triggered when WP mission in progress (s)
 } failsafeConfig_t;
 
 PG_DECLARE(failsafeConfig_t, failsafeConfig);

From 5aa9990c4208d4b510cb4bcb552079c5deaf4f68 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 18 Jun 2022 16:59:01 +0100
Subject: [PATCH 4/6] update

---
 src/main/flight/failsafe.c | 21 ++++++++++++---------
 src/main/flight/failsafe.h |  1 +
 2 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 2c666c1588..31cb772feb 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -67,7 +67,7 @@
 
 static failsafeState_t failsafeState;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 3);
 
 PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT,                                   // 0.5 sec
@@ -82,6 +82,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT,                     // No minimum distance for failsafe by default
     .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure
     .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // Time delay before Failsafe triggered during WP mission (s)
+    .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // WP mode failsafe procedure initiation delay
 );
 
 typedef enum {
@@ -336,16 +337,14 @@ static bool failsafeCheckStickMotion(void)
 
 static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
 {
-    static timeMs_t wpModeDelayedFailsafeStart = 0;
-    if ((FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
-        if (!wpModeDelayedFailsafeStart) {
-            wpModeDelayedFailsafeStart = millis();
+    if ((FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && failsafeConfig()->failsafe_mission_delay) {
+        if (!failsafeState.wpModeDelayedFailsafeStart) {
+            failsafeState.wpModeDelayedFailsafeStart = millis();
+            return FAILSAFE_PROCEDURE_NONE;
         } else {
-            if (failsafeConfig()->failsafe_mission_delay == -1 ||
-                (millis() - wpModeDelayedFailsafeStart <  (MILLIS_PER_SECOND * (uint16_t)failsafeConfig()->failsafe_mission_delay))) {
+            if ((millis() - failsafeState.wpModeDelayedFailsafeStart < (MILLIS_PER_SECOND * (uint16_t)failsafeConfig()->failsafe_mission_delay)) ||
+                failsafeConfig()->failsafe_mission_delay == -1) {
                 return FAILSAFE_PROCEDURE_NONE;
-            } else {
-                wpModeDelayedFailsafeStart = 0;
             }
         }
     }
@@ -403,6 +402,7 @@ void failsafeUpdateState(void)
                             failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
                         } else {
                             failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
+                            failsafeState.wpModeDelayedFailsafeStart = 0;
                         }
                         reprocessState = true;
                     }
@@ -458,6 +458,9 @@ void failsafeUpdateState(void)
                 if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
                     failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
                     reprocessState = true;
+                } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) {  // trigger new failsafe procedure if changed
+                    failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
+                    reprocessState = true;
                 }
                 break;
 
diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h
index fcac289ad7..7cd88acd59 100644
--- a/src/main/flight/failsafe.h
+++ b/src/main/flight/failsafe.h
@@ -148,6 +148,7 @@ typedef struct failsafeState_s {
     timeMs_t landingShouldBeFinishedAt;
     timeMs_t receivingRxDataPeriod;         // period for the required period of valid rxData
     timeMs_t receivingRxDataPeriodPreset;   // preset for the required period of valid rxData
+    timeMs_t wpModeDelayedFailsafeStart;    // waypoint mission delayed failsafe timer start time
     failsafeProcedure_e activeProcedure;
     failsafePhase_e phase;
     failsafeRxLinkState_e rxLinkState;

From df892fb9efbf4d3e0b7a18b2d16b4b4a999b3880 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 18 Jun 2022 17:20:40 +0100
Subject: [PATCH 5/6] Update failsafe.c

---
 src/main/flight/failsafe.c | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 31cb772feb..cf2e35efbc 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -82,7 +82,6 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT,                     // No minimum distance for failsafe by default
     .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure
     .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // Time delay before Failsafe triggered during WP mission (s)
-    .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // WP mode failsafe procedure initiation delay
 );
 
 typedef enum {
@@ -341,11 +340,9 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
         if (!failsafeState.wpModeDelayedFailsafeStart) {
             failsafeState.wpModeDelayedFailsafeStart = millis();
             return FAILSAFE_PROCEDURE_NONE;
-        } else {
-            if ((millis() - failsafeState.wpModeDelayedFailsafeStart < (MILLIS_PER_SECOND * (uint16_t)failsafeConfig()->failsafe_mission_delay)) ||
-                failsafeConfig()->failsafe_mission_delay == -1) {
-                return FAILSAFE_PROCEDURE_NONE;
-            }
+        } else if ((millis() - failsafeState.wpModeDelayedFailsafeStart < (MILLIS_PER_SECOND * (uint16_t)failsafeConfig()->failsafe_mission_delay)) ||
+                   failsafeConfig()->failsafe_mission_delay == -1) {
+            return FAILSAFE_PROCEDURE_NONE;
         }
     }
 

From 099ef4b559f605c535b9a69d4201a432ef5fce71 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 26 Aug 2022 18:08:50 +0100
Subject: [PATCH 6/6] Update setting

---
 docs/Settings.md           | 2 +-
 src/main/fc/settings.yaml  | 2 +-
 src/main/flight/failsafe.c | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index e162703c97..184d6ea9cb 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -904,7 +904,7 @@ What failsafe procedure to initiate in Stage 2 when craft is closer to home than
 
 ### failsafe_mission_delay
 
-Applies when a WP mission is in progress. Sets the time delay in seconds between failsafe activating and the defined Failsafe procedure triggering. If set to -1 the Failsafe procedure won't be triggered at all and the mission will continue until the end.
+Applies if failsafe occurs when a WP mission is in progress. Sets the time delay in seconds between failsafe occurring and the selected failsafe procedure activating. If set to -1 the failsafe procedure won't activate at all and the mission will continue until the end.
 
 | Default | Min | Max |
 | --- | --- | --- |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 5727716794..77595722a5 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -928,7 +928,7 @@ groups:
         default_value: "DROP"
         table: failsafe_procedure
       - name: failsafe_mission_delay
-        description: "Applies when a WP mission is in progress. Sets the time delay in seconds between failsafe activating and the defined Failsafe procedure triggering. If set to -1 the Failsafe procedure won't be triggered at all and the mission will continue until the end."
+        description: "Applies if failsafe occurs when a WP mission is in progress. Sets the time delay in seconds between failsafe occurring and the selected failsafe procedure activating. If set to -1 the failsafe procedure won't activate at all and the mission will continue until the end."
         default_value: 0
         min: -1
         max: 600
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index cf2e35efbc..585a9ee079 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -81,7 +81,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_stick_motion_threshold = SETTING_FAILSAFE_STICK_THRESHOLD_DEFAULT,
     .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT,                     // No minimum distance for failsafe by default
     .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure
-    .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // Time delay before Failsafe triggered during WP mission (s)
+    .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // Time delay before Failsafe activated during WP mission (s)
 );
 
 typedef enum {