From d422abc0cb212f33147526e308aec31296d55b5b Mon Sep 17 00:00:00 2001
From: coleschon <coleperschon@gmail.com>
Date: Tue, 31 Oct 2023 20:41:32 -0700
Subject: [PATCH] AP_TECS: improve velRateMin scaling wrt airspeed

---
 libraries/AP_TECS/AP_TECS.cpp | 16 +++++++++++++---
 libraries/AP_TECS/AP_TECS.h   |  7 ++++---
 2 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index f5892d6858572..48f848c832078 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -470,11 +470,19 @@ void AP_TECS::_update_speed_demand(void)
     // Constrain speed demand, taking into account the load factor
     _TAS_dem = constrain_float(_TAS_dem, _TASmin, _TASmax);
 
+    // Determine the true cruising airspeed (m/s)
+    const float TAScruise = 0.01f * (float)aparm.airspeed_cruise_cm * _ahrs.get_EAS2TAS();
+
     // calculate velocity rate limits based on physical performance limits
     // provision to use a different rate limit if bad descent or underspeed condition exists
-    // Use 50% of maximum energy rate to allow margin for total energy contgroller
+    // Use 50% of maximum energy rate on gain, 90% on dissipation to allow margin for total energy controller
     const float velRateMax = 0.5f * _STEdot_max / _TAS_state;
-    const float velRateMin = 0.5f * _STEdot_min / _TAS_state;
+    // Maximum permissible rate of deceleration value at max airspeed
+    const float velRateNegMax = 0.9f * _STEdot_neg_max / _TASmax;
+    // Maximum permissible rate of deceleration value at cruise speed
+    const float velRateNegCruise = 0.9f * _STEdot_min / TAScruise;
+    // Linear interpolation between velocity rate at cruise and max speeds, capped at those speeds
+    const float velRateMin = linear_interpolate(velRateNegMax, velRateNegCruise, _TAS_state, _TASmax, TAScruise);
     const float TAS_dem_previous = _TAS_dem_adj;
 
     // Apply rate limit
@@ -1163,10 +1171,12 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
 
 void AP_TECS::_update_STE_rate_lim(void)
 {
-    // Calculate Specific Total Energy Rate Limits
+    // Calculate Specific Total Energy Rate Limits & deceleration rate bounds
+	// Keep the 50% energy margin from the original velRate calculation for now
     // This is a trivial calculation at the moment but will get bigger once we start adding altitude effects
     _STEdot_max = _climb_rate_limit * GRAVITY_MSS;
     _STEdot_min = - _minSinkRate * GRAVITY_MSS;
+    _STEdot_neg_max = - _maxSinkRate * GRAVITY_MSS;
 }
 
 
diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
index 4b83d133fa2df..b392a0bfc555b 100644
--- a/libraries/AP_TECS/AP_TECS.h
+++ b/libraries/AP_TECS/AP_TECS.h
@@ -349,9 +349,10 @@ class AP_TECS {
     // pitch demand before limiting
     float _pitch_dem_unc;
 
-    // Maximum and minimum specific total energy rate limits
-    float _STEdot_max;
-    float _STEdot_min;
+    // Specific total energy rate limits
+    float _STEdot_max;     // Specific total energy rate gain at cruise airspeed & THR_MAX (m/s/s)
+    float _STEdot_min;     // Specific total energy rate loss at cruise airspeed & THR_MIN (m/s/s)
+    float _STEdot_neg_max; // Specific total energy rate loss at max airspeed & THR_MIN (m/s/s)
 
     // Maximum and minimum floating point throttle limits
     float _THRmaxf;