diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b2939557ad..db8ce2d4d4 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -45,6 +45,7 @@ enum failsafe_action_long { FS_ACTION_LONG_GLIDE = 2, FS_ACTION_LONG_PARACHUTE = 3, FS_ACTION_LONG_AUTO = 4, + FS_ACTION_LONG_AUTO_SATISFY_JUMP_REPEATS = 5, }; // type of stick mixing enabled diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 64127c633c..552cde7574 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -145,6 +145,10 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason set_mode(mode_fbwa, reason); } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) { set_mode(mode_auto, reason); + } else if (g.fs_action_long == FS_ACTION_LONG_AUTO_SATISFY_JUMP_REPEATS) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Marking all loops as satisfied"); + mission.satisfy_jump_repeats(); + set_mode(mode_auto, reason); } else { set_mode(mode_rtl, reason); } @@ -194,6 +198,10 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason set_mode(mode_fbwa, reason); } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) { set_mode(mode_auto, reason); + } else if (g.fs_action_long == FS_ACTION_LONG_AUTO_SATISFY_JUMP_REPEATS) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Marking all loops as satisfied"); + mission.satisfy_jump_repeats(); + set_mode(mode_auto, reason); } else if (g.fs_action_long == FS_ACTION_LONG_RTL) { set_mode(mode_rtl, reason); } @@ -202,6 +210,10 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason case Mode::Number::RTL: if (g.fs_action_long == FS_ACTION_LONG_AUTO) { set_mode(mode_auto, reason); + } else if (g.fs_action_long == FS_ACTION_LONG_AUTO_SATISFY_JUMP_REPEATS) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Marking all loops as satisfied"); + mission.satisfy_jump_repeats(); + set_mode(mode_auto, reason); } break; #if HAL_QUADPLANE_ENABLED