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 b1119589e7
commit c3545be8a1
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 // calculate velocity rate limits based on physical performance limits
// provision to use a different rate limit if bad descent or underspeed condition exists // 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 // Use 50% of maximum energy rate to allow margin for total energy contgroller
float velRateMax = 0.5f * _STEdot_max / _TAS_state; const float velRateMax = 0.5f * _STEdot_max / _TAS_state;
float velRateMin = 0.5f * _STEdot_min / _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 // 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; _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; _TAS_rate_dem = velRateMin;
} }
else else
{ {
_TAS_rate_dem = (_TAS_dem - TAS_dem_previous) / dt;
_TAS_dem_adj = _TAS_dem; _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. // Constrain speed demand again to protect against bad values on initialisation.
_TAS_dem_adj = constrain_float(_TAS_dem_adj, _TASmin, _TASmax); _TAS_dem_adj = constrain_float(_TAS_dem_adj, _TASmin, _TASmax);
_TAS_dem_last = _TAS_dem;
} }
void AP_TECS::_update_height_demand(void) void AP_TECS::_update_height_demand(void)
@ -908,7 +911,6 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_hgt_dem_adj = _hgt_dem_adj_last; _hgt_dem_adj = _hgt_dem_adj_last;
_hgt_dem_prev = _hgt_dem_adj_last; _hgt_dem_prev = _hgt_dem_adj_last;
_hgt_dem_in_old = _hgt_dem_adj_last; _hgt_dem_in_old = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem;
_TAS_dem_adj = _TAS_dem; _TAS_dem_adj = _TAS_dem;
_flags.underspeed = false; _flags.underspeed = false;
_flags.badDescent = false; _flags.badDescent = false;
@ -922,7 +924,6 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_hgt_dem_adj_last = hgt_afe; _hgt_dem_adj_last = hgt_afe;
_hgt_dem_adj = _hgt_dem_adj_last; _hgt_dem_adj = _hgt_dem_adj_last;
_hgt_dem_prev = _hgt_dem_adj_last; _hgt_dem_prev = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem;
_TAS_dem_adj = _TAS_dem; _TAS_dem_adj = _TAS_dem;
_flags.underspeed = false; _flags.underspeed = false;
_flags.badDescent = false; _flags.badDescent = false;

3
libraries/AP_TECS/AP_TECS.h
View File

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