Для прохождения всех точек можно после взлета включать автопилот. Не очень красиво, но помогает избегать этой ситуации.