forked from Archive/PX4-Autopilot
TECS: Fix bug (underspeed-condition did not have any effect on throttle)
This commit is contained in:
parent
2e33683630
commit
47367aeed5
|
@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
|
|||
// Calculate throttle demand
|
||||
// If underspeed condition is set, then demand full throttle
|
||||
if (_underspeed) {
|
||||
_throttle_dem_unc = 1.0f;
|
||||
_throttle_dem = 1.0f;
|
||||
|
||||
} else {
|
||||
// Calculate gain scaler from specific energy error to throttle
|
||||
|
@ -363,11 +363,11 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
|
|||
} else {
|
||||
_throttle_dem = ff_throttle;
|
||||
}
|
||||
}
|
||||
|
||||
// Constrain throttle demand
|
||||
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
|
||||
}
|
||||
}
|
||||
|
||||
void TECS::_detect_bad_descent(void)
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue