Plane: fixed a bug in LOITER_TURNS in quadplanes

if NAV_LOITER_TURNS is used with Q_GUIDED_MODE=1 then we would orbit
forever. This ensures we do exit the loiter
This commit is contained in:
Andrew Tridgell 2020-02-11 14:36:02 +11:00
parent fa2a9d5f30
commit bf544136c1

View File

@ -2971,6 +2971,11 @@ bool QuadPlane::guided_mode_enabled(void)
if (plane.control_mode != &plane.mode_guided && plane.control_mode != &plane.mode_auto) {
return false;
}
if (plane.control_mode == &plane.mode_auto &&
plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_LOITER_TURNS) {
// loiter turns is a fixed wing only operation
return false;
}
return guided_mode != 0;
}