dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

tecs: change pitch on climbout #559 (ported from ardupilot)

This commit is contained in:
Thomas Gubler 2013-12-23 13:18:05 +01:00 committed by Lorenz Meier
parent b345202ae7
commit c4c652e9c6
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -404,10 +404,18 @@ void TECS::_update_pitch(void)
// 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
// 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;
}
_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
// Calculate pitch demand from specific energy balance signals
_pitch_dem_unc = (temp + _integ7_state) / gainInv;