fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 07:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

tecs: fix limiting of pitch integrator input, better comments and structure

Browse Source 
- when limiting the pitch integrator input the value was related to
a quantity with different units (specific energy error rate vs delta pitch)
- once the unconstrained pitch demand is larger / smaller than the max/min
allowed pitch angle the integrator input should only be allowed to drag
the integrator into the direction leading to less pitch demand violation

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-10-18 09:19:52 +02:00
committed by Paul Riseborough
parent 12ddf9d25e
commit 7106881cbc
1 changed files with 13 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/external_lgpl/tecs/tecs.cpp
+13 -7
View File
@@ -447,29 +447,35 @@ void TECS::_update_pitch(void)
_SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
_SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
// Calculate factor relating an error in specific energy to a desired delta pitch angle
float gainInv = _integ5_state * _timeConst * CONSTANTS_ONE_G;
// Calculate integrator state, constraining input if pitch limits are exceeded
float integ7_input = _SEB_error * _integGain;
if (_pitch_dem_unc > _PITCHmaxf) {
integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
integ7_input = min(integ7_input, 0.0f);
// integ7_input = min(integ7_input, (_PITCHmaxf - _pitch_dem_unc) * gainInv / _DT);
} else if (_pitch_dem_unc < _PITCHminf) {
integ7_input = max(integ7_input, _PITCHminf - _pitch_dem_unc);
integ7_input = max(integ7_input, 0.0f);
// integ7_input = max(integ7_input, (_PITCHminf - _pitch_dem_unc) * gainInv / _DT);
}
_integ7_state = _integ7_state + integ7_input * _DT;
// Apply max and min values for integrator state that will allow for no more than
// 5deg of saturation. This allows for some pitch variation due to gusts before the
// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
float gainInv = _integ5_state * _timeConst * CONSTANTS_ONE_G;
float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
if (_climbOutDem) {
temp += _PITCHminf * gainInv;
}
// Apply max and min values for integrator state that will allow for no more than
// 5deg of saturation. This allows for some pitch variation due to gusts before the
// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
float integ7_err_min = (gainInv * (_PITCHminf - math::radians(5.0f))) - temp;
float integ7_err_max = (gainInv * (_PITCHmaxf + math::radians(5.0f))) - temp;
_integ7_state = constrain(_integ7_state, integ7_err_min, integ7_err_max);