diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index 17b6f25ec9..effac7f984 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/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() diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index 23ff9ed4bc..c3b8a47ec7 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/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