From beecca8da9c191405d12bdd6b98557b3177bed94 Mon Sep 17 00:00:00 2001
From: Ryan Beall <ryan.beall@krausaerospace.com>
Date: Mon, 15 Nov 2021 11:22:15 -0800
Subject: [PATCH] AP_TECS: Connsider throttle saturation in height demand
 limiting

---
 libraries/AP_TECS/AP_TECS.cpp | 34 ++++++++++++++++++++++++++++++----
 libraries/AP_TECS/AP_TECS.h   |  8 ++++++++
 2 files changed, 38 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index bbd463518d..8b6480440a 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -518,6 +518,18 @@ void AP_TECS::_update_height_demand(void)
         _flare_counter = 0;
     }
 
+    // Don't allow height demand to get too far ahead of the vehicles current height
+    // if vehicle is unable to follow the demanded climb or descent
+    const bool max_climb_condition = (_pitch_dem_unc > _PITCHmaxf || _thr_clip_status == ThrClipStatus::MAX) &&
+                                     !(_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
+    const bool max_descent_condition = _pitch_dem_unc < _PITCHminf || _thr_clip_status == ThrClipStatus::MIN;
+    if (max_climb_condition && _hgt_dem > _hgt_dem_prev) {
+        _hgt_dem = _hgt_dem_prev;
+    } else if (max_descent_condition && _hgt_dem < _hgt_dem_prev) {
+        _hgt_dem = _hgt_dem_prev;
+    }
+    _hgt_dem_prev = _hgt_dem;
+
     // for landing approach we will predict ahead by the time constant
     // plus the lag produced by the first order filter. This avoids a
     // lagged height demand while constantly descending which causes
@@ -670,8 +682,16 @@ void AP_TECS::_update_throttle_with_airspeed(void)
         }
         _throttle_dem = (_STE_error + STEdot_error * throttle_damp) * K_STE2Thr + ff_throttle;
 
-        // Constrain throttle demand
-        _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
+        // Constrain throttle demand and record clipping
+        if (_throttle_dem > _THRmaxf) {
+            _thr_clip_status = ThrClipStatus::MAX;
+            _throttle_dem = _THRmaxf;
+        } else if (_throttle_dem < _THRminf) {
+            _thr_clip_status = ThrClipStatus::MIN;
+            _throttle_dem = _THRminf;
+        } else {
+            _thr_clip_status = ThrClipStatus::NONE;
+        }
 
         float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _THRmaxf);
 
@@ -721,8 +741,14 @@ void AP_TECS::_update_throttle_with_airspeed(void)
         _throttle_dem = _throttle_dem + _integTHR_state;
     }
 
-    // Constrain throttle demand
-    _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
+    // Constrain throttle demand and record clip status
+    if (_throttle_dem > _THRmaxf) {
+        _thr_clip_status = ThrClipStatus::MAX;
+        _throttle_dem = _THRmaxf;
+    } else if (_throttle_dem < _THRminf) {
+        _thr_clip_status = ThrClipStatus::MIN;
+        _throttle_dem = _THRminf;
+    }
 }
 
 float AP_TECS::_get_i_gain(void)
diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
index 3cef58211f..845a00b373 100644
--- a/libraries/AP_TECS/AP_TECS.h
+++ b/libraries/AP_TECS/AP_TECS.h
@@ -324,6 +324,14 @@ private:
     float _PITCHmaxf;
     float _PITCHminf;
 
+    // 1 if throttle is clipping at max value, -1 if clipping at min value, 0 otherwise
+    enum class ThrClipStatus  : int8_t {
+        MIN  = -1,
+        NONE =  0,
+        MAX  =  1,
+    };
+    ThrClipStatus _thr_clip_status;
+
     // Specific energy quantities
     float _SPE_dem;
     float _SKE_dem;