Plane: ensure we always eventually capture a loiter

This commit is contained in:
Andrew Tridgell 2016-03-18 13:47:34 +11:00
parent eb89b5bbb6
commit f0eddd6366
1 changed files with 2 additions and 1 deletions

View File

@ -155,7 +155,8 @@ void Plane::update_loiter(uint16_t radius)
}
if (loiter.start_time_ms == 0) {
if (nav_controller->reached_loiter_target()) {
if (nav_controller->reached_loiter_target() ||
auto_state.wp_proportion > 1) {
// we've reached the target, start the timer
loiter.start_time_ms = millis();
if (control_mode == GUIDED) {