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

AP_TECS: remove lag from height demand in landing

Browse Source 
this predicts ahead the height demand for landing, where we have a
continuous demanded descent. This removes the effect of the lag
introduced by the height demand filters
This commit is contained in:
Andrew Tridgell 2014-12-18 09:58:12 +11:00
parent eb13395967
commit 597273cfff
1 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_TECS/AP_TECS.cpp
View File

@ -372,6 +372,7 @@ void AP_TECS::_update_height_demand(void)
{
// Apply 2 point moving average to demanded height
// This is required because height demand is only updated at 5Hz
float hgt_dem_orig = _hgt_dem;
_hgt_dem = 0.5f * (_hgt_dem + _hgt_dem_in_old);
_hgt_dem_in_old = _hgt_dem;
@ -410,7 +411,18 @@ void AP_TECS::_update_height_demand(void)
_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
_flare_counter = 0;
}
// for landing approach we will predict ahead by the time constant
// plus the lag produced by the first order filter. This avoids a
// lagged height demand while constantly descending which causes
// us to consistently be above the desired glide slope. This will
// be replaced with a better zero-lag filter in the future.
float new_hgt_dem = _hgt_dem_adj;
if (_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage == FLIGHT_LAND_FINAL) {
new_hgt_dem += (_hgt_dem_adj - _hgt_dem_adj_last)*10.0f*(timeConstant()+1);
}
_hgt_dem_adj_last = _hgt_dem_adj;
_hgt_dem_adj = new_hgt_dem;
}
void AP_TECS::_detect_underspeed(void)
@ -674,8 +686,6 @@ void AP_TECS::_update_pitch(void)
// Constrain pitch demand
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
// Rate limit the pitch demand to comply with specified vertical
// acceleration limit
float ptchRateIncr = _DT * _vertAccLim / _integ5_state;