Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: auto navigates while stopped

This commit is contained in:
Randy Mackay 2023-11-24 10:10:11 +09:00
parent 62dfe0f04b
commit d88dfa4428
1 changed files with 7 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Rover/mode_auto.cpp
View File

@ -82,21 +82,15 @@ void ModeAuto::update()
switch (_submode) {
case SubMode::WP:
{
// check if we've reached the destination
if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) {
// update navigation controller
navigate_to_waypoint();
} else {
// we have reached the destination so stay here
if (rover.is_boat()) {
if (!start_loiter()) {
start_stop();
}
} else {
start_stop();
}
// boats loiter once the waypoint is reached
if (rover.is_boat() && g2.wp_nav.reached_destination() && !g2.wp_nav.is_fast_waypoint()) {
start_loiter();
// update distance to destination
_distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
} else {
// update navigation controller
navigate_to_waypoint();
}
break;
}