From e2f3cb7ee8d6b2af5e7905e4a9a917db22b44e78 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Mon, 17 Feb 2020 23:45:52 +0000 Subject: [PATCH] Plane: Support added for DO_LAND_START FS fix --- ArduPlane/events.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 3a8cadaad0..ed0f4f5c3d 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -164,6 +164,11 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) case Failsafe_Action_Land: if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland) { // never stop a landing if we were already committed + if (plane.mission.is_best_land_sequence()) { + // continue mission as it will reach a landing in less distance + plane.mission.set_in_landing_sequence_flag(true); + break; + } if (plane.mission.jump_to_landing_sequence()) { plane.set_mode(mode_auto, ModeReason::BATTERY_FAILSAFE); break; @@ -173,6 +178,11 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) case Failsafe_Action_RTL: if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland && !quadplane.in_vtol_land_sequence()) { // never stop a landing if we were already committed + if (g.rtl_autoland == 2 && plane.mission.is_best_land_sequence()) { + // continue mission as it will reach a landing in less distance + plane.mission.set_in_landing_sequence_flag(true); + break; + } set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE); aparm.throttle_cruise.load(); }