AP_TECS: Add option for speed to increase up to FBW max during descent

magicrub · Apr 1, 2024 · 79b091e · 79b091e
1 parent 16b7838
commit 79b091e
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 2 deletions.
diff --git 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),
 
@@ -459,6 +459,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,
@@ -525,9 +530,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
@@ -200,7 +200,8 @@ class AP_TECS {
     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;
@@ -337,6 +338,9 @@ class AP_TECS {
 
         // 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;
@@ -391,6 +395,7 @@ class AP_TECS {
     // 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;