diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index d9199f4b4e..45ebb76737 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1631,6 +1631,8 @@ bool QuadPlane::in_vtol_auto(void) return true; case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_TIME: + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_LOITER_TO_ALT: return plane.auto_state.vtol_loiter; default: return false; @@ -1814,8 +1816,19 @@ void QuadPlane::vtol_position_controller(void) // now height control switch (poscontrol.state) { case QPOS_POSITION1: - case QPOS_POSITION2: - if (plane.control_mode == QRTL || plane.control_mode == GUIDED) { + case QPOS_POSITION2: { + bool vtol_loiter_auto = false; + if (plane.control_mode == AUTO) { + switch (plane.mission.get_current_nav_cmd().id) { + case MAV_CMD_NAV_LOITER_UNLIM: + case MAV_CMD_NAV_LOITER_TIME: + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_LOITER_TO_ALT: + vtol_loiter_auto = true; + break; + } + } + if (plane.control_mode == QRTL || plane.control_mode == GUIDED || vtol_loiter_auto) { plane.ahrs.get_position(plane.current_loc); float target_altitude = plane.next_WP_loc.alt; if (poscontrol.slow_descent) { @@ -1832,6 +1845,7 @@ void QuadPlane::vtol_position_controller(void) pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false); } break; + } case QPOS_LAND_DESCEND: { float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); @@ -1957,6 +1971,8 @@ void QuadPlane::control_auto(const Location &loc) case MAV_CMD_NAV_VTOL_LAND: case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_TIME: + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_LOITER_TO_ALT: vtol_position_controller(); break; default: