Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_TECS : Reduce tendency to overspeed during climbout 

The old code relies on the action of the integrator to raise the nose above the minimum pitch angle which depending on the model and the tuning can be too slow to correct. Biasing the pitch angle to climbout minimum will reduce the taken before an overspeed condition is corrected.
This commit is contained in:
Paul Riseborough 2013-10-16 17:19:26 +11:00 committed by Andrew Tridgell
parent ab1bb6886c
commit 585d61b960
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_TECS/AP_TECS.cpp
View File

@ -515,9 +515,16 @@ void AP_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 * GRAVITY_MSS);
float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
_integ7_state = constrain_float(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF)
{
temp += _PITCHminf * gainInv;
}
_integ7_state = constrain_float(_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;