From 52ae093a97dc81c0a8320c980da70eaa47e6bd57 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tomp@AionRobotics.com>
Date: Wed, 5 Jun 2019 11:23:29 -0700
Subject: [PATCH] AP_Soaring: add reason to exit Thermal loiter

---
 libraries/AP_Soaring/AP_Soaring.cpp | 33 +++++++++++++++++++----------
 libraries/AP_Soaring/AP_Soaring.h   | 10 ++++++++-
 2 files changed, 31 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp
index 507c950d67..61d5c5d943 100644
--- a/libraries/AP_Soaring/AP_Soaring.cpp
+++ b/libraries/AP_Soaring/AP_Soaring.cpp
@@ -171,20 +171,31 @@ bool SoaringController::check_thermal_criteria()
             && _vario.alt > alt_min);
 }
 
-bool SoaringController::check_cruise_criteria()
-{
-    float thermalability = (_ekf.X[0]*expf(-powf(_loiter_rad / _ekf.X[1], 2))) - EXPECTED_THERMALLING_SINK;
-    float alt = _vario.alt;
 
-    if (soar_active && (AP_HAL::micros64() - _thermal_start_time_us) > ((unsigned)min_thermal_s * 1e6) && thermalability < McCready(alt)) {
-        gcs().send_text(MAV_SEVERITY_INFO, "Thermal weak, recommend quitting: W %f R %f th %f alt %f Mc %f", (double)_ekf.X[0], (double)_ekf.X[1], (double)thermalability, (double)alt, (double)McCready(alt));
-        return true;
-    } else if (soar_active && (alt>alt_max || alt<alt_min)) {
-        gcs().send_text(MAV_SEVERITY_ALERT, "Out of allowable altitude range, beginning cruise. Alt = %f", (double)alt);
-        return true;
+SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
+{
+    if (!soar_active) {
+        return SOARING_DISABLED;
     }
 
-    return false;
+    const float alt = _vario.alt;
+
+    if (alt > alt_max) {
+        gcs().send_text(MAV_SEVERITY_ALERT, "Reached upper altitude, beginning cruise. Alt = %f2", (double)alt);
+        return ALT_TOO_HIGH;
+    } else if (alt < alt_min) {
+        gcs().send_text(MAV_SEVERITY_ALERT, "Reached lower altitude, beginning cruise. Alt = %f2", (double)alt);
+        return ALT_TOO_LOW;
+    } else if ((AP_HAL::micros64() - _thermal_start_time_us) > ((unsigned)min_thermal_s * 1e6)) {
+        const float thermalability = (_ekf.X[0]*expf(-powf(_loiter_rad / _ekf.X[1], 2))) - EXPECTED_THERMALLING_SINK;
+        const float mcCreadyAlt = McCready(alt);
+        if (thermalability < mcCreadyAlt) {
+            gcs().send_text(MAV_SEVERITY_INFO, "Thermal weak, recommend quitting: W %f2 R %f2 th %f2 alt %f2 Mc %f2", (double)_ekf.X[0], (double)_ekf.X[1], (double)thermalability, (double)alt, (double)mcCreadyAlt);
+            return THERMAL_WEAK;
+        }
+    }
+
+    return THERMAL_GOOD_TO_KEEP_LOITERING;
 }
 
 bool SoaringController::check_init_thermal_criteria()
diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h
index 2c63903977..d9f65348d1 100644
--- a/libraries/AP_Soaring/AP_Soaring.h
+++ b/libraries/AP_Soaring/AP_Soaring.h
@@ -69,12 +69,20 @@ protected:
 public:
     SoaringController(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms);
 
+    enum LoiterStatus {
+        SOARING_DISABLED,
+        ALT_TOO_HIGH,
+        ALT_TOO_LOW,
+        THERMAL_WEAK,
+        THERMAL_GOOD_TO_KEEP_LOITERING,
+    };
+
     // this supports the TECS_* user settable parameters
     static const struct AP_Param::GroupInfo var_info[];
     void get_target(Location & wp);
     bool suppress_throttle();
     bool check_thermal_criteria();
-    bool check_cruise_criteria();
+    LoiterStatus check_cruise_criteria();
     bool check_init_thermal_criteria();
     void init_thermalling();
     void init_cruising();