AP_Soaring: Add EXIT_COMMANDED status used when pilot changes switch position when thermalling.

libraries/AP_Soaring/AP_Soaring.cpp

@@ -198,7 +198,9 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria(Vector2
     LoiterStatus result = LoiterStatus::GOOD_TO_KEEP_LOITERING;
     const float alt = _vario.alt;
 
-    if (alt > alt_max) {
+    if (_exit_commanded) {
+        result = LoiterStatus::EXIT_COMMANDED;
+    } else if (alt > alt_max) {
         result = LoiterStatus::ALT_TOO_HIGH;
         if (result != _cruise_criteria_msg_last) {
             gcs().send_text(MAV_SEVERITY_ALERT, "Reached upper alt = %dm", (int16_t)alt);
@@ -279,6 +281,8 @@ void SoaringController::init_thermalling()
 
     _position_x_filter.reset(_ekf.X[2]);
     _position_y_filter.reset(_ekf.X[3]);
+
+    _exit_commanded = false;
 }
 
 void SoaringController::init_cruising()
@@ -393,10 +397,18 @@ void SoaringController::update_active_state()
                 // It's enabled, but wasn't on the last loop.
                 gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, manual mode changes.");
                 set_throttle_suppressed(true);
+
+                // We changed mode - if we're in LOITER this means we should exit gracefully.
+                // This has no effect if we're cruising as it is reset on thermal entry.
+                _exit_commanded = true;
                 break;
             case ActiveStatus::AUTO_MODE_CHANGE:
                 gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Enabled, automatic mode changes.");
                 set_throttle_suppressed(true);
+
+                // We changed mode - if we're in LOITER this means we should exit gracefully.
+                // This has no effect if we're cruising as it is reset on thermal entry.
+                _exit_commanded = true;
                 break;
         }
     }

libraries/AP_Soaring/AP_Soaring.h

@@ -81,7 +81,8 @@ public:
         THERMAL_WEAK,
         ALT_LOST,
         DRIFT_EXCEEDED,
-        GOOD_TO_KEEP_LOITERING
+        GOOD_TO_KEEP_LOITERING,
+        EXIT_COMMANDED,
     };
 
     enum class ActiveStatus {
@@ -133,4 +134,6 @@ private:
     ActiveStatus _pilot_desired_state = ActiveStatus::AUTO_MODE_CHANGE;
 
     ActiveStatus active_state() const;
+
+    bool _exit_commanded;
 };