From 86c240465412fd39c03033155cefb11b6c279c41 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 May 2022 18:31:15 +1000 Subject: [PATCH] Plane: increased safety of guided -> auto quadplane takeoff when we arm in guided mode then enter a special guided_wait_takeoff state. We keep motors suppressed until one of the following happens 1) disarm 2) guided takeoff command 3) change to AUTO with a takeoff waypoint as first nav waypoint 4) change to another mode while in this state we don't go to throttle unlimited, and will refuse a change to AUTO mode if the first waypoint is not a takeoff. If we try to switch to RTL then we will instead use QLAND This state is needed to cope with the takeoff sequence used by QGC on common controllers such as the MX16, which do this on a "takeoff" swipe: - changes mode to GUIDED - arms - changes mode to AUTO --- ArduPlane/AP_Arming.cpp | 10 ++++++++++ ArduPlane/mode_auto.cpp | 10 ++++++++++ ArduPlane/mode_qrtl.cpp | 7 +++++++ ArduPlane/mode_rtl.cpp | 9 +++++++-- ArduPlane/quadplane.cpp | 17 ++++++++++++++++- ArduPlane/quadplane.h | 24 ++++++++++++++++++++++++ 6 files changed, 74 insertions(+), 3 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 1dd60ba43a..5ff6f88b42 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -179,6 +179,16 @@ bool AP_Arming_Plane::quadplane_checks(bool display_failure) } } + if ((plane.control_mode == &plane.mode_auto) && !plane.mission.starts_with_takeoff_cmd()) { + check_failed(display_failure,"missing takeoff waypoint"); + ret = false; + } + + if (plane.control_mode == &plane.mode_rtl) { + check_failed(display_failure,"in RTL mode"); + ret = false; + } + /* Q_ASSIST_SPEED really should be enabled for all quadplanes except tailsitters */ diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 9b6332b059..436abdd08e 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -4,6 +4,16 @@ bool ModeAuto::_enter() { #if HAL_QUADPLANE_ENABLED + // check if we should refuse auto mode due to a missing takeoff in + // guided_wait_takeoff state + if (plane.previous_mode == &plane.mode_guided && + quadplane.guided_wait_takeoff_on_mode_enter) { + if (!plane.mission.starts_with_takeoff_cmd()) { + gcs().send_text(MAV_SEVERITY_ERROR,"Takeoff waypoint required"); + return false; + } + } + if (plane.quadplane.available() && plane.quadplane.enable == 2) { plane.auto_state.vtol_mode = true; } else { diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 9ddbd45851..1c59174461 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -5,6 +5,13 @@ bool ModeQRTL::_enter() { + // treat QRTL as QLAND if we are in guided wait takeoff state, to cope + // with failsafes during GUIDED->AUTO takeoff sequence + if (plane.quadplane.guided_wait_takeoff_on_mode_enter) { + plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL); + return true; + } + // use do_RTL() to setup next_WP_loc plane.do_RTL(plane.home.alt + quadplane.qrtl_alt*100UL); plane.prev_WP_loc = plane.current_loc; diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index 7c59ac1f99..a24ea10b65 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -8,10 +8,15 @@ bool ModeRTL::_enter() plane.rtl.done_climb = false; #if HAL_QUADPLANE_ENABLED plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL; -#endif + + // treat RTL as QLAND if we are in guided wait takeoff state, to cope + // with failsafes during GUIDED->AUTO takeoff sequence + if (plane.quadplane.guided_wait_takeoff_on_mode_enter) { + plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL); + return true; + } // do not check if we have reached the loiter target if switching from loiter this will trigger as the nav controller has not yet proceeded the new destination -#if HAL_QUADPLANE_ENABLED switch_QRTL(false); #endif diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 61e221731c..8288469939 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1266,6 +1266,10 @@ void QuadPlane::set_armed(bool armed) return; } motors->armed(armed); + + if (plane.control_mode == &plane.mode_guided) { + guided_wait_takeoff = armed; + } } @@ -1793,6 +1797,10 @@ void QuadPlane::update_throttle_suppression(void) return; } + if (guided_wait_takeoff) { + goto idle_state; + } + /* if the users throttle is above zero then allow motors to run if the user has unset the "check throttle zero when arming" @@ -1832,7 +1840,8 @@ void QuadPlane::update_throttle_suppression(void) if (plane.control_mode == &plane.mode_auto && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) { return; } - + +idle_state: // motors should be in the spin when armed state to warn user they could become active set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors->set_throttle(0); @@ -3506,6 +3515,7 @@ bool QuadPlane::do_user_takeoff(float takeoff_altitude) set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); guided_start(); guided_takeoff = true; + guided_wait_takeoff = false; if ((options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) { ahrs.set_takeoff_expected(true); } @@ -4082,6 +4092,11 @@ void QuadPlane::mode_enter(void) poscontrol.xy_correction.zero(); poscontrol.velocity_match.zero(); poscontrol.last_velocity_match_ms = 0; + + // clear guided takeoff wait on any mode change, but remember the + // state for special behaviour + guided_wait_takeoff_on_mode_enter = guided_wait_takeoff; + guided_wait_takeoff = false; } #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 64182b897e..ef47aa1ca3 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -403,6 +403,30 @@ private: // are we in a guided takeoff? bool guided_takeoff:1; + /* if we arm in guided mode when we arm then go into a "waiting + for takeoff command" state. In this state we are waiting for + one of the following: + + 1) disarm + 2) guided takeoff command + 3) change to AUTO with a takeoff waypoint as first nav waypoint + 4) change to another mode + + while in this state we don't go to throttle unlimited, and will + refuse a change to AUTO mode if the first waypoint is not a + takeoff. If we try to switch to RTL then we will instead use + QLAND + + This state is needed to cope with the takeoff sequence used + by QGC on common controllers such as the MX16, which do this on a "takeoff" swipe: + + - changes mode to GUIDED + - arms + - changes mode to AUTO + */ + bool guided_wait_takeoff; + bool guided_wait_takeoff_on_mode_enter; + struct { // time when motors reached lower limit uint32_t lower_limit_start_ms;