mirror of https://github.com/ArduPilot/ardupilot
When RTLing, force home as the final location for loiter.
This commit is contained in:
parent
5b31f3ba27
commit
abb426aec3
|
@ -1751,6 +1751,10 @@ static void update_navigation()
|
|||
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
|
||||
// if loiter_timer value > 0, we are set to trigger auto_land or approach after 20 seconds
|
||||
set_mode(LOITER);
|
||||
// force loitering above home
|
||||
next_WP.lat = home.lat;
|
||||
next_WP.lng = home.lng;
|
||||
|
||||
if(g.rtl_land_enabled || failsafe)
|
||||
loiter_timer = millis();
|
||||
else
|
||||
|
|
Loading…
Reference in New Issue