diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-15 07:43:17 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-15 07:43:17 +0100 |
commit | f3cd83e804e8fffc5e71e4d6443729184b3e7909 (patch) | |
tree | 13ac8af24705fe5f8107f3f46fd989e49fdd00d4 /src/lib/external_lgpl/tecs/tecs.cpp | |
parent | 9612514a3f59cfedbd7b29c9e4f30c1edf1c7345 (diff) | |
parent | cd72f564eff13d831d6773e85818b65f708d3323 (diff) | |
download | px4-firmware-f3cd83e804e8fffc5e71e4d6443729184b3e7909.tar.gz px4-firmware-f3cd83e804e8fffc5e71e4d6443729184b3e7909.tar.bz2 px4-firmware-f3cd83e804e8fffc5e71e4d6443729184b3e7909.zip |
Merged master into mixer unit tests branch
Diffstat (limited to 'src/lib/external_lgpl/tecs/tecs.cpp')
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 8 |
1 files changed, 8 insertions, 0 deletions
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a733ef1d2..1d5c85699 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -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; |