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

Copter: smooth safertl return journey using fast waypoints

This commit is contained in:
Randy Mackay 2017-09-06 13:34:08 +09:00
parent 6684e3de56
commit ed0203e53c
1 changed files with 3 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/control_safe_rtl.cpp
View File

@ -77,13 +77,16 @@ void Copter::safe_rtl_path_follow_run()
if (wp_nav->reached_wp_destination()) {
Vector3f next_point;
if (g2.safe_rtl.pop_point(next_point)) {
bool fast_waypoint = true;
if (g2.safe_rtl.get_num_points() == 0) {
// this is the very last point, add 2m to the target alt and move to pre-land state
next_point.z -= 2.0f;
safe_rtl_state = SafeRTL_PreLandPosition;
fast_waypoint = false;
}
// send target to waypoint controller
wp_nav->set_wp_destination_NED(next_point);
wp_nav->set_fast_waypoint(fast_waypoint);
} else {
// this can only happen if we fail to get the semaphore which should never happen but just in case, land
safe_rtl_state = SafeRTL_PreLandPosition;