Browse Source

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
c415-sdk
Andrew Tridgell 5 years ago
parent
commit
0bfab20bd0
  1. 5
      ArduPlane/quadplane.cpp

5
ArduPlane/quadplane.cpp

@ -3015,6 +3015,11 @@ bool QuadPlane::guided_mode_enabled(void)
if (plane.control_mode != &plane.mode_guided && plane.control_mode != &plane.mode_auto) { if (plane.control_mode != &plane.mode_guided && plane.control_mode != &plane.mode_auto) {
return false; 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; return guided_mode != 0;
} }

Loading…
Cancel
Save