diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp
index 25d6afb039..4feff10568 100644
--- a/ArduPlane/soaring.cpp
+++ b/ArduPlane/soaring.cpp
@@ -74,6 +74,7 @@ void Plane::update_soaring() {
         const char* strTooHigh = "Soaring: Too high, restoring ";
         const char* strTooLow =  "Soaring: Too low, restoring ";
         const char* strTooWeak = "Soaring: Thermal ended, restoring ";
+        const char* strTooFar  = "Soaring: Drifted too far, restoring ";
 
         // Exit as soon as thermal state estimate deteriorates and we're lined up to next target
         switch (previous_mode->mode_number()){
@@ -97,6 +98,10 @@ void Plane::update_soaring() {
                     gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_fbwb.name());
                     set_mode(mode_fbwb, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
                     break;
+                case SoaringController::LoiterStatus::DRIFT_EXCEEDED:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_fbwb.name());
+                    set_mode(mode_fbwb, ModeReason::SOARING_DRIFT_EXCEEDED);
+                    break;
                 } // switch louterStatus
             break;
             }
@@ -122,6 +127,10 @@ void Plane::update_soaring() {
                     gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_cruise.name());
                     set_mode(mode_cruise, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
                     break;
+                case SoaringController::LoiterStatus::DRIFT_EXCEEDED:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_cruise.name());
+                    set_mode(mode_cruise, ModeReason::SOARING_DRIFT_EXCEEDED);
+                    break;
             } // switch loiterStatus
             cruise_state = cruise;
             set_target_altitude_current();
@@ -131,7 +140,7 @@ void Plane::update_soaring() {
         case Mode::Number::AUTO: {
             //Get the lat/lon of next Nav waypoint after this one:
             AP_Mission::Mission_Command current_nav_cmd = mission.get_current_nav_cmd();;
-            const bool headingLinedupToWP = isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location);
+            const bool headingLinedupToWP = plane.mode_loiter.isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location);
             if (!headingLinedupToWP) {
                 break;
             }
@@ -149,6 +158,10 @@ void Plane::update_soaring() {
                         gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_auto.name());
                         set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
                     break;
+                case SoaringController::LoiterStatus::DRIFT_EXCEEDED:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_auto.name());
+                    set_mode(mode_auto, ModeReason::SOARING_DRIFT_EXCEEDED);
+                    break;
                 } // switch loiterStatus
             break;
             } // case AUTO
diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp
index 4aae75db28..adaf9bac1e 100644
--- a/libraries/AP_Soaring/AP_Soaring.cpp
+++ b/libraries/AP_Soaring/AP_Soaring.cpp
@@ -121,7 +121,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ENABLE_CH", 15, SoaringController, soar_active_ch, 0),
 
-    // @Param: MAX_DEV
+    // @Param: MAX_DRIFT
     // @DisplayName: (Optional) Maximum drift distance to allow when thermalling.
     // @Description: The previous mode will be restored if the horizontal distance to the thermalling start location exceeds this value. 0 to disable.
     // @Range: 0 1000
@@ -216,8 +216,8 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
             if (result != _cruise_criteria_msg_last) {
                 gcs().send_text(MAV_SEVERITY_INFO, "Not climbing");
             }
-        } else if (max_drift && (powf(position.x - _thermal_start_pos.x, 2) + powf(position.y - _thermal_start_pos.y, 2)) > powf(max_drift,2)) {
-            result = DEVIATION_EXCEEDED;
+        } else if (!(max_drift<0) && (powf(position.x - _thermal_start_pos.x, 2) + powf(position.y - _thermal_start_pos.y, 2)) > powf(max_drift,2)) {
+            result = DRIFT_EXCEEDED;
             if (result != _cruise_criteria_msg_last) {
                 gcs().send_text(MAV_SEVERITY_INFO, "Drifted too far");
             }
diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h
index e6c4ad6944..3f94ae2aa6 100644
--- a/libraries/AP_Soaring/AP_Soaring.h
+++ b/libraries/AP_Soaring/AP_Soaring.h
@@ -78,7 +78,7 @@ public:
         ALT_TOO_LOW,
         THERMAL_WEAK,
         ALT_LOST,
-        DEVIATION_EXCEEDED,
+        DRIFT_EXCEEDED,
         THERMAL_GOOD_TO_KEEP_LOITERING
     } LoiterStatus;
 
diff --git a/libraries/AP_Vehicle/ModeReason.h b/libraries/AP_Vehicle/ModeReason.h
index f993c71275..84838b9c35 100644
--- a/libraries/AP_Vehicle/ModeReason.h
+++ b/libraries/AP_Vehicle/ModeReason.h
@@ -55,4 +55,5 @@ enum class ModeReason : uint8_t {
   AUTOROTATION_BAILOUT,
   SOARING_ALT_TOO_HIGH,
   SOARING_ALT_TOO_LOW,
+  SOARING_DRIFT_EXCEEDED,
 };