From 32ba248c7308685f5be6bf91cd27f6e118947beb Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Tue, 13 Nov 2018 17:40:40 +0900 Subject: [PATCH] Rover: add continue-in-auto option to failsafe --- APMrover2/Parameters.cpp | 8 ++++---- APMrover2/defines.h | 14 ++++++++++++++ APMrover2/failsafe.cpp | 8 +++++++- 3 files changed, 25 insertions(+), 5 deletions(-) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index ccd879dcd8..d4f09923a8 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -144,9 +144,9 @@ const AP_Param::Info Rover::var_info[] = { // @Param: FS_THR_ENABLE // @DisplayName: Throttle Failsafe Enable // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel to a low value. This can be used to detect the RC transmitter going out of range. Failsafe will be triggered when the throttle channel goes below the FS_THR_VALUE for FS_TIMEOUT seconds. - // @Values: 0:Disabled,1:Enabled + // @Values: 0:Disabled,1:Enabled,2:Enabled Continue with Mission in Auto // @User: Standard - GSCALAR(fs_throttle_enabled, "FS_THR_ENABLE", 1), + GSCALAR(fs_throttle_enabled, "FS_THR_ENABLE", FS_THR_ENABLED), // @Param: FS_THR_VALUE // @DisplayName: Throttle Failsafe Value @@ -159,9 +159,9 @@ const AP_Param::Info Rover::var_info[] = { // @Param: FS_GCS_ENABLE // @DisplayName: GCS failsafe enable // @Description: Enable ground control station telemetry failsafe. When enabled the Rover will execute the FS_ACTION when it fails to receive MAVLink heartbeat packets for FS_TIMEOUT seconds. - // @Values: 0:Disabled,1:Enabled + // @Values: 0:Disabled,1:Enabled,2:Enabled Continue with Mission in Auto // @User: Standard - GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE", 0), + GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE", FS_GCS_DISABLED), // @Param: FS_CRASH_CHECK // @DisplayName: Crash check action diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 520778ec26..89bb2aec51 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -77,6 +77,20 @@ // subsystem specific error codes -- crash checker #define ERROR_CODE_CRASH_CHECK_CRASH 1 +// radio failsafe enum (FS_THR_ENABLE parameter) +enum fs_thr_enable { + FS_THR_DISABLED = 0, + FS_THR_ENABLED, + FS_THR_ENABLED_CONTINUE_MISSION, +}; + +// gcs failsafe enum (FS_GCS_ENABLE parameter) +enum fs_gcs_enable { + FS_GCS_DISABLED = 0, + FS_GCS_ENABLED, + FS_GCS_ENABLED_CONTINUE_MISSION, +}; + enum fs_crash_action { FS_CRASH_DISABLE = 0, FS_CRASH_HOLD = 1, diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp index 76d12ee271..ff9d294b66 100644 --- a/APMrover2/failsafe.cpp +++ b/APMrover2/failsafe.cpp @@ -73,7 +73,12 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) // clear rc overrides RC_Channels::clear_overrides(); - switch (g.fs_action) { + if ((control_mode == &mode_auto) && + ((failsafe_type == FAILSAFE_EVENT_THROTTLE && g.fs_throttle_enabled == FS_THR_ENABLED_CONTINUE_MISSION) || + (failsafe_type == FAILSAFE_EVENT_GCS && g.fs_gcs_enabled == FS_GCS_ENABLED_CONTINUE_MISSION))) { + // continue with mission in auto mode + } else { + switch (g.fs_action) { case Failsafe_Action_None: break; case Failsafe_Action_RTL: @@ -96,6 +101,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) set_mode(mode_hold, MODE_REASON_FAILSAFE); } break; + } } } }