TECS: only use positive throttle range for throttle response calculat…

…ions (ArduPilot#37) * Add OSD input throttle element (ArduPilot#31) * Plane: add arming switch safety (ArduPilot#30) * use positive throttle range for tecs throttle respose calculations Co-authored-by: Michel Pastor <shellixyz@users.noreply.github.com>
shellixyz · Jun 15, 2022 · cd7181d · cd7181d
1 parent e8a8de6
commit cd7181d
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 11 deletions.
diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
@@ -28,7 +28,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
 
     // @Param: SINK_MIN
     // @DisplayName: Minimum Sink Rate (metres/sec)
-    // @Description: Minimum sink rate when at THR_MIN and TRIM_ARSPD_CM.
+    // @Description: Minimum sink rate when at THR_MIN (or zero throttle when THR_MIN < 0) and TRIM_ARSPD_CM.
     // @Increment: 0.1
     // @Range: 0.1 10.0
     // @User: Standard
@@ -108,7 +108,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
 
     // @Param: SINK_MAX
     // @DisplayName: Maximum Descent Rate (metres/sec)
-    // @Description: Maximum demanded descent rate. Do not set higher than the vertical speed the aircraft can maintain at THR_MIN, TECS_PITCH_MIN, and ARSPD_FBW_MAX.
+    // @Description: Maximum demanded descent rate. Do not set higher than the vertical speed the aircraft can maintain at THR_MIN (or zero throttle when THR_MIN < 0), TECS_PITCH_MIN, and ARSPD_FBW_MAX.
     // @Increment: 0.1
     // @Range: 0.0 20.0
     // @User: Standard
@@ -662,7 +662,7 @@ void AP_TECS::_update_throttle_with_airspeed(void)
     {
         // Calculate gain scaler from specific energy error to throttle
         // (_STEdot_max - _STEdot_min) / (_THRmaxf - _THRminf) is the derivative of STEdot wrt throttle measured across the max allowed throttle range.
-        float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min) / (_THRmaxf - _THRminf));
+        float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min) / (_THRmaxf - _THRminf_clipped_to_zero));
 
         // Calculate feed-forward throttle
         float ff_throttle = 0;
@@ -673,7 +673,7 @@ void AP_TECS::_update_throttle_with_airspeed(void)
         // drag increase during turns.
         float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
         STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
-        ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
+        ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf_clipped_to_zero);
 
         // Calculate PD + FF throttle
         float throttle_damp = _thrDamp;
@@ -693,12 +693,10 @@ void AP_TECS::_update_throttle_with_airspeed(void)
             _thr_clip_status = ThrClipStatus::NONE;
         }
 
-        float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _THRmaxf);
-
         // Calculate integrator state upper and lower limits
         // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
         // Additionally constrain the integrator state amplitude so that the integrator comes off limits faster.
-        float maxAmp = 0.5f*(_THRmaxf - THRminf_clipped_to_zero);
+        float maxAmp = 0.5f * (_THRmaxf - _THRminf_clipped_to_zero);
         float integ_max = constrain_float((_THRmaxf - _throttle_dem + 0.1f),-maxAmp,maxAmp);
         float integ_min = constrain_float((_THRminf - _throttle_dem - 0.1f),-maxAmp,maxAmp);
 
@@ -729,7 +727,7 @@ void AP_TECS::_update_throttle_with_airspeed(void)
         }
 
         if (throttle_slewrate != 0) {
-            float thrRateIncr = _DT * (_THRmaxf - THRminf_clipped_to_zero) * throttle_slewrate * 0.01f;
+            float thrRateIncr = _DT * (_THRmaxf - _THRminf_clipped_to_zero) * throttle_slewrate * 0.01f;
 
             _throttle_dem = constrain_float(_throttle_dem,
                                             _last_throttle_dem - thrRateIncr,
@@ -807,7 +805,7 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge)
     // drag increase during turns.
     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);
+    _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf_clipped_to_zero);
 }
 
 void AP_TECS::_detect_bad_descent(void)
@@ -1052,10 +1050,11 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
     } else {
         _THRmaxf  = aparm.throttle_max * 0.01f;
     }
-    _THRminf  = aparm.throttle_min * 0.01f;
+    _THRminf = aparm.throttle_min * 0.01f;
+    _THRminf_clipped_to_zero = MAX(_THRminf, 0);
 
     // min of 1% throttle range to prevent a numerical error
-    _THRmaxf = MAX(_THRmaxf, _THRminf+0.01);
+    _THRmaxf = MAX(_THRmaxf, _THRminf_clipped_to_zero + 0.01);
 
     // work out the maximum and minimum pitch
     // if TECS_PITCH_{MAX,MIN} isn't set then use

diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
@@ -325,6 +325,7 @@ class AP_TECS {
     // Maximum and minimum floating point throttle limits
     float _THRmaxf;
     float _THRminf;
+    float _THRminf_clipped_to_zero;
 
     // Maximum and minimum floating point pitch limits
     float _PITCHmaxf;