From 07501f634c25a320b02fd0ec58188cf16ab6ae59 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Mon, 19 Feb 2024 20:17:42 +1100
Subject: [PATCH] AP_TECS: Add option for speed to increase up to FBW max
 during descent

---
 libraries/AP_TECS/AP_TECS.cpp | 14 +++++++++++++-
 libraries/AP_TECS/AP_TECS.h   |  7 ++++++-
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index dd9c8b428e..7ef4241798 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -246,7 +246,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: Extra TECS options
     // @Description: This allows the enabling of special features in the speed/height controller.
-    // @Bitmask: 0:GliderOnly
+    // @Bitmask: 0:GliderOnly,1:AllowDescentSpeedup
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 28, AP_TECS, _options, 0),
 
@@ -458,6 +458,11 @@ void AP_TECS::_update_speed(float DT)
 
 void AP_TECS::_update_speed_demand(void)
 {
+    if (_options & OPTION_DESCENT_SPEEDUP) {
+        // Allow demanded speed to  go to maximum when descending at maximum descent rate
+        _TAS_dem = _TAS_dem + (_TASmax - _TAS_dem) * _sink_fraction;
+    }
+
     // Set the airspeed demand to the minimum value if an underspeed condition exists
     // or a bad descent condition exists
     // This will minimise the rate of descent resulting from an engine failure,
@@ -532,9 +537,16 @@ void AP_TECS::_update_height_demand(void)
         // Limit height rate of change
         if ((hgt_dem - _hgt_dem_rate_ltd) > (_climb_rate_limit * _DT)) {
             _hgt_dem_rate_ltd = _hgt_dem_rate_ltd + _climb_rate_limit * _DT;
+            _sink_fraction = 0.0f;
         } else if ((hgt_dem - _hgt_dem_rate_ltd) < (-_sink_rate_limit * _DT)) {
             _hgt_dem_rate_ltd = _hgt_dem_rate_ltd - _sink_rate_limit * _DT;
+            _sink_fraction = 1.0f;
         } else {
+            if (is_negative(hgt_dem - _hgt_dem_rate_ltd)) {
+                _sink_fraction = (hgt_dem - _hgt_dem_rate_ltd) / (-_sink_rate_limit * _DT);
+            } else {
+                _sink_fraction = 0.0f;
+            }
             _hgt_dem_rate_ltd = hgt_dem;
         }
 
diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
index b392a0bfc5..50b91a882c 100644
--- a/libraries/AP_TECS/AP_TECS.h
+++ b/libraries/AP_TECS/AP_TECS.h
@@ -197,7 +197,8 @@ private:
     AP_Float _hgt_dem_tconst;
 
     enum {
-        OPTION_GLIDER_ONLY=(1<<0)
+        OPTION_GLIDER_ONLY=(1<<0),
+        OPTION_DESCENT_SPEEDUP=(1<<1)
     };
 
     AP_Float _pitch_ff_v0;
@@ -334,6 +335,9 @@ private:
 
         // true when a reset of airspeed and height states to current is performed on this frame
         bool reset:1;
+
+        // true when we are allowing the plane to speed up on descent to maintain the target descent rate
+        bool speedup:1;
     };
     union {
         struct flags _flags;
@@ -389,6 +393,7 @@ private:
     // used to scale max climb and sink limits to match vehicle ability
     float _max_climb_scaler;
     float _max_sink_scaler;
+    float _sink_fraction;
 
     // Specific energy error quantities
     float _STE_error;