Browse Source

AP_TECS: gain scaler K_STE2Thr multiplies by (THRmax - THRmin)

Makes both feed forward and feed-back consistent
master
JU Han 8 years ago committed by Francisco Ferreira
parent
commit
eebef857f1
  1. 3
      libraries/AP_TECS/AP_TECS.cpp

3
libraries/AP_TECS/AP_TECS.cpp

@ -608,7 +608,8 @@ void AP_TECS::_update_throttle_with_airspeed(void) @@ -608,7 +608,8 @@ void AP_TECS::_update_throttle_with_airspeed(void)
else
{
// Calculate gain scaler from specific energy error to throttle
float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min));
// (_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));
// Calculate feed-forward throttle
float ff_throttle = 0;

Loading…
Cancel
Save