Add option to continue mission on FS

docs/Cli.md

@@ -305,6 +305,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  failsafe_lights | ON | Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. |
 |  failsafe_lights_flash_period | 1000 | Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]. |
 |  failsafe_lights_flash_on_time | 100 | Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]. |
+|  failsafe_mission | ON | 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 |
 |  rx_min_usec  | 885 | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc. |
 |  rx_max_usec  | 2115 | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc. |
 |  rx_nosignal_throttle  | HOLD | Defines behavior of throttle channel after signal loss is detected and until `failsafe_procedure` kicks in. Possible values - `HOLD` and `DROP`. |

src/main/fc/settings.yaml

@@ -514,6 +514,8 @@ groups:
         max: 65000
       - name: failsafe_min_distance_procedure
         table: failsafe_procedure
+      - name: failsafe_mission
+        type: bool
 
   - name: PG_LIGHTS_CONFIG
     type: lightsConfig_t

src/main/flight/failsafe.c

@@ -64,21 +64,22 @@
 
 static failsafeState_t failsafeState;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 1);
 
 PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
-    .failsafe_delay = 5,                // 0.5 sec
-    .failsafe_recovery_delay = 5,       // 0.5 seconds (plus 200ms explicit delay)
-    .failsafe_off_delay = 200,          // 20sec
-    .failsafe_throttle = 1000,          // default throttle off.
-    .failsafe_throttle_low_delay = 0,   // default throttle low delay for "just disarm" on failsafe condition
-    .failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING,            // default full failsafe procedure
-    .failsafe_fw_roll_angle = -200,     // 20 deg left
-    .failsafe_fw_pitch_angle = 100,     // 10 deg dive (yes, positive means dive)
-    .failsafe_fw_yaw_rate = -45,        // 45 deg/s left yaw (left is negative, 8s for full turn)
+    .failsafe_delay = 5,                                                // 0.5 sec
+    .failsafe_recovery_delay = 5,                                       // 0.5 seconds (plus 200ms explicit delay)
+    .failsafe_off_delay = 200,                                          // 20sec
+    .failsafe_throttle = 1000,                                          // default throttle off.
+    .failsafe_throttle_low_delay = 0,                                   // default throttle low delay for "just disarm" on failsafe condition
+    .failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING,              // default full failsafe procedure
+    .failsafe_fw_roll_angle = -200,                                     // 20 deg left
+    .failsafe_fw_pitch_angle = 100,                                     // 10 deg dive (yes, positive means dive)
+    .failsafe_fw_yaw_rate = -45,                                        // 45 deg/s left yaw (left is negative, 8s for full turn)
     .failsafe_stick_motion_threshold = 50,
-    .failsafe_min_distance = 0,            // No minimum distance for failsafe by default
-    .failsafe_min_distance_procedure = FAILSAFE_PROCEDURE_DROP_IT   // default minimum distance failsafe procedure
+    .failsafe_min_distance = 0,                                         // No minimum distance for failsafe by default
+    .failsafe_min_distance_procedure = FAILSAFE_PROCEDURE_DROP_IT,      // default minimum distance failsafe procedure
+    .failsafe_mission = true,                                           // Enable failsafe in WP mode or not
 );
 
 typedef enum {
@@ -400,6 +401,10 @@ void failsafeUpdateState(void)
                 } else {
                     uint8_t failsafe_procedure_to_use = failsafeConfig()->failsafe_procedure;
 
+                    if (FLIGHT_MODE(NAV_WP_MODE) && !failsafeConfig()->failsafe_mission) {
+                        failsafe_procedure_to_use = FAILSAFE_PROCEDURE_NONE;
+                    }
+
                     // Craft is closer than minimum failsafe procedure distance (if set to non-zero)
                     // GPS must also be working, and home position set
                     if ((failsafeConfig()->failsafe_min_distance > 0) &&

src/main/flight/failsafe.h

@@ -30,19 +30,20 @@
 
 
 typedef struct failsafeConfig_s {
-    uint16_t failsafe_throttle;             // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
-    uint16_t failsafe_throttle_low_delay;   // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND)
-    uint8_t failsafe_delay;                 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
-    uint8_t failsafe_recovery_delay;        // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10)
-    uint8_t failsafe_off_delay;             // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
-    uint8_t failsafe_procedure;             // selected full failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)
+    uint16_t failsafe_throttle;                 // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
+    uint16_t failsafe_throttle_low_delay;       // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND)
+    uint8_t failsafe_delay;                     // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
+    uint8_t failsafe_recovery_delay;            // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10)
+    uint8_t failsafe_off_delay;                 // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
+    uint8_t failsafe_procedure;                 // selected full failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)
 
-    int16_t failsafe_fw_roll_angle;         // Settings to be applies during "LAND" procedure on a fixed-wing
+    int16_t failsafe_fw_roll_angle;             // Settings to be applies during "LAND" procedure on a fixed-wing
     int16_t failsafe_fw_pitch_angle;
     int16_t failsafe_fw_yaw_rate;
     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_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
 } failsafeConfig_t;
 
 PG_DECLARE(failsafeConfig_t, failsafeConfig);