Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: stop at waypoint depending upon next command

Browse Source
This commit is contained in:
Randy Mackay 2019-02-25 13:10:39 +09:00
parent 92b68d516b
commit afea0353c8
2 changed files with 26 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/mode.h
View File

@ -392,7 +392,6 @@ private:
#endif #endif
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
// Loiter control // Loiter control
uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds)
uint32_t loiter_time; // How long have we been loitering - The start time in millis uint32_t loiter_time; // How long have we been loitering - The start time in millis

27
ArduCopter/mode_auto.cpp
View File

@ -1096,8 +1096,33 @@ void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
// if no delay as well as not final waypoint set the waypoint as "fast" // if no delay as well as not final waypoint set the waypoint as "fast"
AP_Mission::Mission_Command temp_cmd; AP_Mission::Mission_Command temp_cmd;
bool fast_waypoint = false;
if (loiter_time_max == 0 && copter.mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) { if (loiter_time_max == 0 && copter.mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
copter.wp_nav->set_fast_waypoint(true);
// whether vehicle should stop at the target position depends upon the next command
switch (temp_cmd.id) {
case MAV_CMD_NAV_WAYPOINT:
case MAV_CMD_NAV_LOITER_UNLIM:
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_LOITER_TIME:
case MAV_CMD_NAV_LAND:
case MAV_CMD_NAV_SPLINE_WAYPOINT:
// if next command's lat, lon is specified then do not slowdown at this waypoint
if ((temp_cmd.content.location.lat != 0) || (temp_cmd.content.location.lng != 0)) {
fast_waypoint = true;
}
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
// do not stop for RTL
fast_waypoint = true;
break;
case MAV_CMD_NAV_TAKEOFF:
default:
// always stop for takeoff commands
// for unsupported commands it is safer to stop
break;
}
copter.wp_nav->set_fast_waypoint(fast_waypoint);
} }
} }