Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_TECS : Improvements and bug fixes to feed-forward throttle demand

Browse Source 
This fixes a bug in the feed-forward throttle that prevented the bank to throttle compensation from working properly during descents.
It also adds bank to throttle compensation to the non-airspeed sensor throttle algorithm.
This commit is contained in:
Paul Riseborough 2013-07-31 20:16:37 +10:00 committed by Andrew Tridgell
parent 2690edfc5f
commit 34f0af25cc
1 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_TECS/AP_TECS.cpp
View File

@ -368,16 +368,9 @@ void AP_TECS::_update_throttle(void)
// Use the demanded rate of change of total energy as the feed-forward demand, but add
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrt((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
if (STEdot_dem >= 0)
{
ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
}
else
{
ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
}
ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
// Calculate PD + FF throttle
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
@ -400,7 +393,7 @@ void AP_TECS::_update_throttle(void)
float integ_min = (_THRminf - _throttle_dem - 0.1f);
// Calculate integrator state, constraining state
// Set integrator to a max throttle value dduring climbout
// Set integrator to a max throttle value during climbout
_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
if (_climbOutDem)
{
@ -444,6 +437,15 @@ void AP_TECS::_update_throttle_option(int16_t throttle_nudge)
{
_throttle_dem = nomThr;
}
// Calculate additional throttle for turn drag compensation including throttle nudging
const Matrix3f &rotMat = _ahrs->get_dcm_matrix();
// Use the demanded rate of change of total energy as the feed-forward demand, but add
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
_throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
}
void AP_TECS::_detect_bad_descent(void)
@ -571,7 +573,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
void AP_TECS::_update_STE_rate_lim(void)
{
// Calculate Specific Total Energy Rate Limits
// This is a tivial calculation at the moment but will get bigger once we start adding altitude effects
// This is a trivial calculation at the moment but will get bigger once we start adding altitude effects
_STEdot_max = _maxClimbRate * GRAVITY_MSS;
_STEdot_min = - _minSinkRate * GRAVITY_MSS;
}