From 085c5c815b706b9acdba8a775de43cb3465aefe9 Mon Sep 17 00:00:00 2001 From: Hwurzburg Date: Sun, 17 Oct 2021 19:32:10 -0500 Subject: [PATCH] Plane: Fix bug with mode return on short fs exit and add FBWB action --- ArduPlane/Parameters.cpp | 4 ++-- ArduPlane/defines.h | 1 + ArduPlane/events.cpp | 15 +++++++++------ 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index a611d99023..ebe927e3e5 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -405,8 +405,8 @@ const AP_Param::Info Plane::var_info[] = { // @Param: FS_SHORT_ACTN // @DisplayName: Short failsafe action - // @Description: The action to take on a short (FS_SHORT_TIMEOUT) failsafe event. A short failsafe event can be triggered either by loss of RC control (see THR_FS_VALUE) or by loss of GCS control (see FS_GCS_ENABL). If in CIRCLE or RTL mode this parameter is ignored. A short failsafe event in stabilization and manual modes will cause a change to CIRCLE mode if FS_SHORT_ACTN is 0 or 1, and a change to FBWA mode if FS_SHORT_ACTN is 2. In all other modes (AUTO, GUIDED and LOITER) a short failsafe event will cause no mode change if FS_SHORT_ACTN is set to 0, will cause a change to CIRCLE mode if set to 1 and will change to FBWA mode if set to 2. Please see the documentation for FS_LONG_ACTN for the behaviour after FS_LONG_TIMEOUT seconds of failsafe. - // @Values: 0:CIRCLE/no change(if already in AUTO|GUIDED|LOITER),1:CIRCLE,2:FBWA,3:Disable + // @Description: The action to take on a short (FS_SHORT_TIMEOUT) failsafe event. A short failsafe event can be triggered either by loss of RC control (see THR_FS_VALUE) or by loss of GCS control (see FS_GCS_ENABL). If in CIRCLE or RTL mode this parameter is ignored. A short failsafe event in stabilization and manual modes will cause a change to CIRCLE mode if FS_SHORT_ACTN is 0 or 1, a change to FBWA mode with zero throttle if FS_SHORT_ACTN is 2, and a change to FBWB mode if FS_SHORT_ACTN is 4. In all other modes (AUTO, GUIDED and LOITER) a short failsafe event will cause no mode change if FS_SHORT_ACTN is set to 0, will cause a change to CIRCLE mode if set to 1, will change to FBWA mode with zero throttle if set to 2, or will change to FBWB if set to 4. Please see the documentation for FS_LONG_ACTN for the behaviour after FS_LONG_TIMEOUT seconds of failsafe. + // @Values: 0:CIRCLE/no change(if already in AUTO|GUIDED|LOITER),1:CIRCLE,2:FBWA at zero throttle,3:Disable,4:FBWB // @User: Standard GSCALAR(fs_action_short, "FS_SHORT_ACTN", FS_ACTION_SHORT_BESTGUESS), diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 2533661a24..fe06002708 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -36,6 +36,7 @@ enum failsafe_action_short { FS_ACTION_SHORT_CIRCLE = 1, FS_ACTION_SHORT_FBWA = 2, FS_ACTION_SHORT_DISABLED = 3, + FS_ACTION_SHORT_FBWB = 4, }; enum failsafe_action_long { diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index b6d1d84d24..93d642d92e 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -35,6 +35,8 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso failsafe.saved_mode_set = true; if(g.fs_action_short == FS_ACTION_SHORT_FBWA) { set_mode(mode_fbwa, reason); + } else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) { + set_mode(mode_fbwb, reason); } else { set_mode(mode_circle, reason); } @@ -183,12 +185,13 @@ void Plane::failsafe_short_off_event(ModeReason reason) // We're back in radio contact gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off: reason=%u", static_cast(reason)); failsafe.state = FAILSAFE_NONE; - - // re-read the switch so we can return to our preferred mode - // -------------------------------------------------------- - if (control_mode == &mode_circle && failsafe.saved_mode_set) { - failsafe.saved_mode_set = false; - set_mode_by_number(failsafe.saved_mode_number, reason); + if(failsafe.saved_mode_set) { //we saved an entry mode..check that our fs mode has not been changed by GCS + if((control_mode == &mode_circle && g.fs_action_short == FS_ACTION_SHORT_CIRCLE) || + (control_mode == &mode_fbwa && g.fs_action_short == FS_ACTION_SHORT_FBWA) || + (control_mode == &mode_fbwb && g.fs_action_short == FS_ACTION_SHORT_FBWB)) { + failsafe.saved_mode_set = false; + set_mode_by_number(failsafe.saved_mode_number, reason); //mode has not been changed while in FS, return to entry mode + } } }