Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 00:58:37 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Fix bug with mode return on short fs exit and add FBWB action

Browse Source
This commit is contained in:
Hwurzburg 2021-10-17 19:32:10 -05:00 committed by Andrew Tridgell
parent d7cc2cb296
commit 085c5c815b
3 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/Parameters.cpp
View File

@ -405,8 +405,8 @@ const AP_Param::Info Plane::var_info[] = {
// @Param: FS_SHORT_ACTN // @Param: FS_SHORT_ACTN
// @DisplayName: Short failsafe action // @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. // @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,3:Disable // @Values: 0:CIRCLE/no change(if already in AUTO|GUIDED|LOITER),1:CIRCLE,2:FBWA at zero throttle,3:Disable,4:FBWB
// @User: Standard // @User: Standard
GSCALAR(fs_action_short, "FS_SHORT_ACTN", FS_ACTION_SHORT_BESTGUESS), GSCALAR(fs_action_short, "FS_SHORT_ACTN", FS_ACTION_SHORT_BESTGUESS),

1
ArduPlane/defines.h
View File

@ -36,6 +36,7 @@ enum failsafe_action_short {
FS_ACTION_SHORT_CIRCLE = 1, FS_ACTION_SHORT_CIRCLE = 1,
FS_ACTION_SHORT_FBWA = 2, FS_ACTION_SHORT_FBWA = 2,
FS_ACTION_SHORT_DISABLED = 3, FS_ACTION_SHORT_DISABLED = 3,
FS_ACTION_SHORT_FBWB = 4,
}; };
enum failsafe_action_long { enum failsafe_action_long {

15
ArduPlane/events.cpp
View File

@ -35,6 +35,8 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
failsafe.saved_mode_set = true; failsafe.saved_mode_set = true;
if(g.fs_action_short == FS_ACTION_SHORT_FBWA) { if(g.fs_action_short == FS_ACTION_SHORT_FBWA) {
set_mode(mode_fbwa, reason); set_mode(mode_fbwa, reason);
} else if (g.fs_action_short == FS_ACTION_SHORT_FBWB) {
set_mode(mode_fbwb, reason);
} else { } else {
set_mode(mode_circle, reason); set_mode(mode_circle, reason);
} }
@ -183,12 +185,13 @@ void Plane::failsafe_short_off_event(ModeReason reason)
// We're back in radio contact // We're back in radio contact
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off: reason=%u", static_cast<unsigned>(reason)); gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off: reason=%u", static_cast<unsigned>(reason));
failsafe.state = FAILSAFE_NONE; failsafe.state = FAILSAFE_NONE;
if(failsafe.saved_mode_set) { //we saved an entry mode..check that our fs mode has not been changed by GCS
// re-read the switch so we can return to our preferred mode 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) ||
if (control_mode == &mode_circle && failsafe.saved_mode_set) { (control_mode == &mode_fbwb && g.fs_action_short == FS_ACTION_SHORT_FBWB)) {
failsafe.saved_mode_set = false; failsafe.saved_mode_set = false;
set_mode_by_number(failsafe.saved_mode_number, reason); set_mode_by_number(failsafe.saved_mode_number, reason); //mode has not been changed while in FS, return to entry mode
}
} }
} }