From e7736fb5e70383dce21a75ae02e0d18462109c10 Mon Sep 17 00:00:00 2001
From: Samuel Tabor <samuelctabor@gmail.com>
Date: Sun, 9 Jun 2019 01:12:43 +0100
Subject: [PATCH] AP_Soaring: Line up on current, not next, nav command.

---
 ArduPlane/soaring.cpp | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp
index 613a4b6c05..c2218873d6 100644
--- a/ArduPlane/soaring.cpp
+++ b/ArduPlane/soaring.cpp
@@ -128,10 +128,9 @@ void Plane::update_soaring() {
 
         case Mode::Number::AUTO: {
             //Get the lat/lon of next Nav waypoint after this one:
-            AP_Mission::Mission_Command next_nav_cmd;
-            const bool nextWpisValid = mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, next_nav_cmd);
-            const bool headingLinedupToWP = nextWpisValid && plane.mode_loiter.isHeadingLinedUp(next_WP_loc, next_nav_cmd.content.location);
-            if (nextWpisValid && !headingLinedupToWP) {
+            AP_Mission::Mission_Command current_nav_cmd = mission.get_current_nav_cmd();;
+            const bool headingLinedupToWP = isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location);
+            if (!headingLinedupToWP) {
                 break;
             }
             switch (loiterStatus) {