Copter: loiter-turns fix

This commit is contained in:
Randy Mackay 2021-04-11 17:42:52 +09:00
parent affff0fc84
commit 6a98ad5a29

View File

@ -1185,7 +1185,6 @@ bool ModeAuto::set_next_wp(const AP_Mission::Mission_Command& current_cmd, const
switch (next_cmd.id) { switch (next_cmd.id) {
case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_WAYPOINT:
case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_UNLIM:
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_LOITER_TIME: { case MAV_CMD_NAV_LOITER_TIME: {
const Location dest_loc = loc_from_cmd(current_cmd, default_loc); const Location dest_loc = loc_from_cmd(current_cmd, default_loc);
const Location next_dest_loc = loc_from_cmd(next_cmd, dest_loc); const Location next_dest_loc = loc_from_cmd(next_cmd, dest_loc);
@ -1200,6 +1199,7 @@ bool ModeAuto::set_next_wp(const AP_Mission::Mission_Command& current_cmd, const
} }
case MAV_CMD_NAV_LAND: case MAV_CMD_NAV_LAND:
// stop because we may change between rel,abs and terrain alt types // stop because we may change between rel,abs and terrain alt types
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_RETURN_TO_LAUNCH: case MAV_CMD_NAV_RETURN_TO_LAUNCH:
case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_TAKEOFF:
// always stop for RTL and takeoff commands // always stop for RTL and takeoff commands