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

AP_TECS: fixed a bug in changes from rate-limited to non-limited airspeed 

The calculation of the non-limited airspeed rate demand used the last
non-limited airspeed, whereas it should have used the last adjusted
value. This led to a single frame spike in airspeed demand, which fed
through to a sudden change in pitch integrator.
This commit is contained in:
Andrew Tridgell 2019-04-01 09:28:42 +11:00
parent b1a8055f20
commit f50a6b59cc
2 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_TECS/AP_TECS.cpp
View File

@ -409,28 +409,31 @@ void AP_TECS::_update_speed_demand(void)
// calculate velocity rate limits based on physical performance limits
// provision to use a different rate limit if bad descent or underspeed condition exists
// Use 50% of maximum energy rate to allow margin for total energy contgroller
float velRateMax = 0.5f * _STEdot_max / _TAS_state;
float velRateMin = 0.5f * _STEdot_min / _TAS_state;
const float velRateMax = 0.5f * _STEdot_max / _TAS_state;
const float velRateMin = 0.5f * _STEdot_min / _TAS_state;
const float TAS_dem_previous = _TAS_dem_adj;
// assume fixed 10Hz call rate
const float dt = 0.1;
// Apply rate limit
if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f))
if ((_TAS_dem - TAS_dem_previous) > (velRateMax * dt))
{
_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
_TAS_dem_adj = TAS_dem_previous + velRateMax * dt;
_TAS_rate_dem = velRateMax;
}
else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f))
else if ((_TAS_dem - TAS_dem_previous) < (velRateMin * dt))
{
_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
_TAS_dem_adj = TAS_dem_previous + velRateMin * dt;
_TAS_rate_dem = velRateMin;
}
else
{
_TAS_rate_dem = (_TAS_dem - TAS_dem_previous) / dt;
_TAS_dem_adj = _TAS_dem;
_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
}
// Constrain speed demand again to protect against bad values on initialisation.
_TAS_dem_adj = constrain_float(_TAS_dem_adj, _TASmin, _TASmax);
_TAS_dem_last = _TAS_dem;
}
void AP_TECS::_update_height_demand(void)
@ -913,7 +916,6 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_hgt_dem_adj = _hgt_dem_adj_last;
_hgt_dem_prev = _hgt_dem_adj_last;
_hgt_dem_in_old = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem;
_TAS_dem_adj = _TAS_dem;
_flags.underspeed = false;
_flags.badDescent = false;
@ -927,7 +929,6 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_hgt_dem_adj_last = hgt_afe;
_hgt_dem_adj = _hgt_dem_adj_last;
_hgt_dem_prev = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem;
_TAS_dem_adj = _TAS_dem;
_flags.underspeed = false;
_flags.badDescent = false;

3
libraries/AP_TECS/AP_TECS.h
View File

@ -226,9 +226,8 @@ private:
float _TASmax;
float _TASmin;
// Current and last true airspeed demand
// Current true airspeed demand
float _TAS_dem;
float _TAS_dem_last;
// Equivalent airspeed demand
float _EAS_dem;