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
This commit is contained in:
Andrew Tridgell 2022-05-16 18:31:15 +10:00
parent 72b5212baf
commit c093c5d945
6 changed files with 74 additions and 3 deletions

View File

@ -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
*/

View File

@ -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 {

View File

@ -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;

View File

@ -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

View File

@ -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"
@ -1833,6 +1841,7 @@ void QuadPlane::update_throttle_suppression(void)
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);
@ -3504,6 +3513,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);
}
@ -4080,6 +4090,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

View File

@ -402,6 +402,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;