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

Rover: fix notification in guided and rtl

This commit is contained in:
Randy Mackay 2019-05-09 19:48:24 +09:00
parent 40f3f414cd
commit d0a39e6114
2 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
APMrover2/mode_guided.cpp
View File

@ -153,6 +153,7 @@ bool ModeGuided::set_desired_location(const struct Location& destination,
// handle guided specific initialisation and logging
_guided_mode = ModeGuided::Guided_WP;
sent_notification = false;
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f));
return true;
}

11
APMrover2/mode_rtl.cpp
View File

@ -35,7 +35,7 @@ bool ModeRTL::_enter()
void ModeRTL::update()
{
// determine if we should keep navigating
if (!g2.wp_nav.reached_destination() || rover.is_boat()) {
if (!g2.wp_nav.reached_destination()) {
// update navigation controller
navigate_to_waypoint();
} else {
@ -45,8 +45,13 @@ void ModeRTL::update()
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
}
// we've reached destination so stop
stop_vehicle();
// we have reached the destination
// boats keep navigating, rovers stop
if (rover.is_boat()) {
navigate_to_waypoint();
} else {
stop_vehicle();
}
// update distance to destination
_distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());