Browse Source

tecs: Remove redundant code

The if() statement was there as provision to do something different with the speed demand derivative limits if an abnormal condition was present, however we have no basis for assigning different limits at this point in time.
master
Paul Riseborough 7 years ago committed by Lorenz Meier
parent
commit
1ddd94f237
  1. 17
      tecs/tecs.cpp

17
tecs/tecs.cpp

@ -206,24 +206,15 @@ void TECS::_update_speed_setpoint()
// Set the airspeed demand to the minimum value if an underspeed or // Set the airspeed demand to the minimum value if an underspeed or
// or a uncontrolled descent condition exists to maximise climb rate // or a uncontrolled descent condition exists to maximise climb rate
if ((_uncommanded_descent_recovery) || (_underspeed_detected)) { if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
_TAS_setpoint = _TAS_min; _TAS_setpoint = _TAS_min;
} }
_TAS_setpoint = constrain(_TAS_setpoint, _TAS_min, _TAS_max); _TAS_setpoint = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
// Apply limits on the demanded rate of change of speed based on physical performance limits // Calculate limits for the demanded rate of change of speed based on physical performance limits
// with a 50% margin to allow the total energy controller to correct for errors. // with a 50% margin to allow the total energy controller to correct for errors.
float velRateMax; float velRateMax = 0.5f * _STE_rate_max / _tas_state;
float velRateMin; float velRateMin = 0.5f * _STE_rate_min / _tas_state;
if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
velRateMax = 0.5f * _STE_rate_max / _tas_state;
velRateMin = 0.5f * _STE_rate_min / _tas_state;
} else {
velRateMax = 0.5f * _STE_rate_max / _tas_state;
velRateMin = 0.5f * _STE_rate_min / _tas_state;
}
_TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max); _TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max);

Loading…
Cancel
Save