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;