From 2d61ceafa17f9b13b1229e013e80ca45f7e39efd Mon Sep 17 00:00:00 2001
From: George Zogopoulos <tailwhipx4@gmail.com>
Date: Thu, 4 Jul 2024 14:19:00 +0200
Subject: [PATCH] AP_TECS: Constrained throttle in airspeedless

---
 libraries/AP_TECS/AP_TECS.cpp | 9 ++++++++-
 libraries/AP_TECS/AP_TECS.h   | 3 +++
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index 9307502caaf0b..b7edd83e9c739 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -823,7 +823,12 @@ void AP_TECS::_update_throttle_with_airspeed(void)
 #endif
     }
 
-    // Constrain throttle demand and record clipping
+    constrain_throttle();
+
+}
+
+// Constrain throttle demand and record clipping
+void AP_TECS::constrain_throttle() {
     if (_throttle_dem > _THRmaxf) {
         _thr_clip_status = clipStatus::MAX;
         _throttle_dem = _THRmaxf;
@@ -914,6 +919,8 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi
     float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
     float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f);
     _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
+
+    constrain_throttle();
 }
 
 void AP_TECS::_detect_bad_descent(void)
diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
index fcd3fe97f9bfe..2c061952b9103 100644
--- a/libraries/AP_TECS/AP_TECS.h
+++ b/libraries/AP_TECS/AP_TECS.h
@@ -458,6 +458,9 @@ class AP_TECS {
     // Update Demanded Throttle Non-Airspeed
     void _update_throttle_without_airspeed(int16_t throttle_nudge, float pitch_trim_deg);
 
+    // Constrain throttle demand and record clipping
+    void constrain_throttle();
+
     // get integral gain which is flight_stage dependent
     float _get_i_gain(void);