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: make flare height demand consistent with height rate demand

Browse Source
This commit is contained in:
priseborough 2014-11-28 18:39:34 +11:00 committed by Andrew Tridgell
parent a0f69e06a7
commit bd97f15d06
2 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_TECS/AP_TECS.cpp
View File

@ -383,10 +383,12 @@ void AP_TECS::_update_height_demand(void)
_hgt_dem_adj_last = _hgt_dem_adj;
// in final landing stage force height rate demand to the
// configured sink rate
// configured sink rate and adjust the demanded height to
// be kinematically consistent with the height rate.
if (_flight_stage == FLIGHT_LAND_FINAL) {
if (_flare_counter == 0) {
_hgt_rate_dem = _climb_rate;
_land_hgt_dem = _hgt_dem_adj;
}
// bring it in over 1s to prevent overshoot
if (_flare_counter < 10) {
@ -395,7 +397,9 @@ void AP_TECS::_update_height_demand(void)
} else {
_hgt_rate_dem = - _land_sink;
}
} else {
_land_hgt_dem += 0.1f * _hgt_rate_dem;
_hgt_dem_adj = _land_hgt_dem;
} else {
_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
_flare_counter = 0;
}

1
libraries/AP_TECS/AP_TECS.h
View File

@ -186,6 +186,7 @@ private:
float _hgt_dem_adj_last;
float _hgt_rate_dem;
float _hgt_dem_prev;
float _initial_land_hgt;
// Speed demand after application of rate limiting
// This is the demand tracked by the TECS control loops