diff --git a/docs/Settings.md b/docs/Settings.md index ca0d7b90d8..184d6ea9cb 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -902,13 +902,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 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 | | --- | --- | --- | -| ON | OFF | ON | +| 0 | -1 | 600 | --- diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 40ad55c2f8..61050073bc 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -191,7 +191,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 f264763e68..77595722a5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -927,10 +927,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 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 - name: PG_LIGHTS_CONFIG type: lightsConfig_t diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 0e761f3bd0..585a9ee079 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -67,13 +67,13 @@ 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 .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 activated during WP mission (s) ); typedef enum { @@ -336,8 +336,14 @@ static bool failsafeCheckStickMotion(void) static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) { - if ((FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && !failsafeConfig()->failsafe_mission) { - return FAILSAFE_PROCEDURE_NONE; + if ((FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && failsafeConfig()->failsafe_mission_delay) { + 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; + } } // Craft is closer than minimum failsafe procedure distance (if set to non-zero) @@ -393,6 +399,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; } @@ -448,6 +455,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 0a118d733d..7cd88acd59 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 + int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s) } failsafeConfig_t; PG_DECLARE(failsafeConfig_t, failsafeConfig); @@ -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;