Plane: allow for new guided destination during guided takeoff

if you set a very high alt for takeoff this is useful
This commit is contained in:
Andrew Tridgell 2021-09-17 08:46:03 +10:00
parent 8bee839931
commit 7547ad53d3
2 changed files with 9 additions and 0 deletions

View File

@ -99,6 +99,11 @@ void Plane::set_guided_WP(void)
auto_state.vtol_loiter = false;
loiter_angle_reset();
#if HAL_QUADPLANE_ENABLED
// cancel pending takeoff
quadplane.guided_takeoff = false;
#endif
}
/*

View File

@ -2011,6 +2011,10 @@ bool QuadPlane::in_vtol_mode(void) const
poscontrol.get_state() > QPOS_APPROACH) {
return true;
}
if (plane.control_mode == &plane.mode_guided &&
guided_takeoff) {
return true;
}
if (in_vtol_auto()) {
if (!plane.auto_state.vtol_loiter || poscontrol.get_state() > QPOS_APPROACH) {
return true;