diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp
index bdf18311b0..10f0842494 100644
--- a/ArduPlane/soaring.cpp
+++ b/ArduPlane/soaring.cpp
@@ -43,6 +43,9 @@ void Plane::update_soaring() {
     }
 
     switch (control_mode->mode_number()) {
+    default:
+        // nothing to do
+        break;
     case Mode::Number::AUTO:
     case Mode::Number::FLY_BY_WIRE_B:
     case Mode::Number::CRUISE:
@@ -50,50 +53,101 @@ void Plane::update_soaring() {
         g2.soaring_controller.update_cruising();
 
         if (g2.soaring_controller.check_thermal_criteria()) {
-            gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering loiter");
+            gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering %s", mode_loiter.name());
             set_mode(mode_loiter, ModeReason::SOARING_THERMAL_DETECTED);
         }
         break;
 
-    case Mode::Number::LOITER:
+    case Mode::Number::LOITER: {
         // Update thermal estimate and check for switch back to AUTO
         g2.soaring_controller.update_thermalling();  // Update estimate
 
-        if (g2.soaring_controller.check_cruise_criteria()) {
-            // Exit as soon as thermal state estimate deteriorates
-            switch (previous_mode->mode_number()) {
-            case Mode::Number::FLY_BY_WIRE_B:
-                gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, entering RTL");
-                set_mode(mode_rtl, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
-                break;
+        const SoaringController::LoiterStatus loiterStatus = g2.soaring_controller.check_cruise_criteria();
 
-            case Mode::Number::CRUISE: {
-                // return to cruise with old ground course
-                CruiseState cruise = cruise_state;
-                gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring CRUISE");
-                set_mode(mode_cruise, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
-                cruise_state = cruise;
-                set_target_altitude_current();
-                break;
-            }
+        if (loiterStatus == SoaringController::LoiterStatus::THERMAL_GOOD_TO_KEEP_LOITERING ||
+            loiterStatus == SoaringController::LoiterStatus::SOARING_DISABLED) {
+            break;
+        }
 
-            case Mode::Number::AUTO:
-                gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring AUTO");
-                set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
-                break;
+        const char* strTooHigh = "Soaring: Too high, restoring ";
+        const char* strTooLow =  "Soaring: Too high, restoring ";
+        const char* strTooWeak = "Soaring: Thermal ended, restoring ";
 
-            default:
+        // Exit as soon as thermal state estimate deteriorates
+        switch (previous_mode->mode_number()) {
+        case Mode::Number::FLY_BY_WIRE_B: {
+            switch (loiterStatus) {
+                case SoaringController::LoiterStatus::ALT_TOO_HIGH:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_fbwb.name());
+                    set_mode(mode_fbwb, ModeReason::SOARING_ALT_TOO_HIGH);
+                    break;
+                default:
+                case SoaringController::LoiterStatus::ALT_TOO_LOW:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooLow, mode_rtl.name());
+                    set_mode(mode_rtl, ModeReason::SOARING_ALT_TOO_LOW);
+                    break;
+                case SoaringController::LoiterStatus::THERMAL_WEAK:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_fbwb.name());
+                    set_mode(mode_fbwb, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
+                    break;
+            } // switch louterStatus
                 break;
+
+        case Mode::Number::CRUISE: {
+            // return to cruise with old ground course
+            const CruiseState cruise = cruise_state;
+            switch (loiterStatus) {
+                case SoaringController::LoiterStatus::ALT_TOO_HIGH:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_cruise.name());
+                    set_mode(mode_cruise, ModeReason::SOARING_ALT_TOO_HIGH);
+                    break;
+                default:
+                case SoaringController::LoiterStatus::ALT_TOO_LOW:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooLow, mode_rtl.name());
+                    set_mode(mode_rtl, ModeReason::SOARING_ALT_TOO_LOW);
+                    break;
+                case SoaringController::LoiterStatus::THERMAL_WEAK:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_cruise.name());
+                    set_mode(mode_cruise, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
+                    break;
+            } // switch loiterStatus
+            cruise_state = cruise;
+            set_target_altitude_current();
+            break;
             }
-        } else {
-            // still in thermal - need to update the wp location
-            g2.soaring_controller.get_target(next_WP_loc);
-        }
-        break;
-    default:
-        // nothing to do
+
+        case Mode::Number::AUTO:
+            switch (loiterStatus) {
+                case SoaringController::LoiterStatus::ALT_TOO_HIGH:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_auto.name());
+                    set_mode(mode_auto, ModeReason::SOARING_ALT_TOO_HIGH);
+                    break;
+                default:
+                case SoaringController::LoiterStatus::ALT_TOO_LOW:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooLow, mode_auto.name());
+                    set_mode(mode_auto, ModeReason::SOARING_ALT_TOO_LOW);
+                    break;
+                case SoaringController::LoiterStatus::THERMAL_WEAK:
+                    gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_auto.name());
+                    set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
+                    break;
+            } // switch loiterStatus
+            break;
+
+        default:
+            break;
+        } // switch previous_mode
         break;
+        } // loiter case
+    }} // switch control_mode
+
+
+    if (control_mode == &mode_loiter) {
+        // still in thermal - need to update the wp location
+        g2.soaring_controller.get_target(next_WP_loc);
+
     }
+
 }
 
 #endif // SOARING_ENABLED
diff --git a/libraries/AP_Vehicle/ModeReason.h b/libraries/AP_Vehicle/ModeReason.h
index d0a4f56001..f993c71275 100644
--- a/libraries/AP_Vehicle/ModeReason.h
+++ b/libraries/AP_Vehicle/ModeReason.h
@@ -53,4 +53,6 @@ enum class ModeReason : uint8_t {
   UNAVAILABLE,
   AUTOROTATION_START,
   AUTOROTATION_BAILOUT,
+  SOARING_ALT_TOO_HIGH,
+  SOARING_ALT_TOO_LOW,
 };