AP_Soaring: Add a check of whether altitude has been lost overall when thermalling.

libraries/AP_Soaring/AP_Soaring.cpp

@@ -199,6 +199,11 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
             if (result != _cruise_criteria_msg_last) {
                 gcs().send_text(MAV_SEVERITY_INFO, "Thermal weak: W %f.3 R %f.3 th %f.1 alt %dm Mc %dm", (double)_ekf.X[0], (double)_ekf.X[1], (double)thermalability, (int32_t)alt, (int32_t)mcCreadyAlt);
             }
+        } else if (alt < _thermal_start_alt) {
+            result = ALT_LOST;
+            if (result != _cruise_criteria_msg_last) {
+                gcs().send_text(MAV_SEVERITY_INFO, "Altitude lost from entry = %dm", (int32_t)_thermal_start_alt);
+            }
         }
     }
 
@@ -246,6 +251,7 @@ void SoaringController::init_thermalling()
 
     _prev_update_time = AP_HAL::micros64();
     _thermal_start_time_us = AP_HAL::micros64();
+    _thermal_start_alt = _vario.alt;
 }
 
 void SoaringController::init_cruising()

libraries/AP_Soaring/AP_Soaring.h

@@ -37,6 +37,9 @@ class SoaringController {
     // store time thermal was entered for hysteresis
     unsigned long _thermal_start_time_us;
 
+    // store altitude thermal was entered as a backup check
+    float _thermal_start_alt;
+
     // store time cruise was entered for hysteresis
     unsigned long _cruise_start_time_us;
 
@@ -72,7 +75,8 @@ public:
         ALT_TOO_HIGH,
         ALT_TOO_LOW,
         THERMAL_WEAK,
-        THERMAL_GOOD_TO_KEEP_LOITERING,
+        ALT_LOST,
+        THERMAL_GOOD_TO_KEEP_LOITERING
     } LoiterStatus;
 
     // this supports the TECS_* user settable parameters