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

Copter: mode_smart_rtl uses peek_point

Browse Source
This commit is contained in:
Randy Mackay 2020-01-08 20:18:39 +09:00
parent 193799346c
commit 204c839ae0
1 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduCopter/mode_smart_rtl.cpp
View File

@ -97,16 +97,21 @@ void ModeSmartRTL::path_follow_run()
// path semaphore.
if (g2.smart_rtl.pop_point(next_point)) {
path_follow_last_pop_fail_ms = 0;
bool fast_waypoint = true;
if (g2.smart_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;
smart_rtl_state = SmartRTL_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 {
// peek at the next point
Vector3f next_next_point;
if (g2.smart_rtl.peek_point(next_next_point)) {
wp_nav->set_wp_destination_NED(next_point, next_next_point);
} else {
// this should never happen but send next point anyway
wp_nav->set_wp_destination_NED(next_point);
}
}
} else if (g2.smart_rtl.get_num_points() == 0) {
// We should never get here; should always have at least
// two points and the "zero points left" is handled above.