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

AP_TECS: slew the hgt_lag_filter when starting a land

This commit is contained in:
Tom Pittenger 2016-07-15 13:38:59 -07:00
parent 2527859b59
commit 7be15be185
2 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_TECS/AP_TECS.cpp
View File

@ -504,7 +504,14 @@ void AP_TECS::_update_height_demand(void)
// be replaced with a better zero-lag filter in the future.
float new_hgt_dem = _hgt_dem_adj;
if (_flags.is_doing_auto_land) {
new_hgt_dem += (_hgt_dem_adj - _hgt_dem_adj_last)*10.0f*(timeConstant()+1);
if (hgt_dem_lag_filter_slew < 1) {
hgt_dem_lag_filter_slew += 0.1f; // increment at 10Hz to gradually apply the compensation at first
} else {
hgt_dem_lag_filter_slew = 1;
}
new_hgt_dem += hgt_dem_lag_filter_slew*(_hgt_dem_adj - _hgt_dem_adj_last)*10.0f*(timeConstant()+1);
} else {
hgt_dem_lag_filter_slew = 0;
}
_hgt_dem_adj_last = _hgt_dem_adj;
_hgt_dem_adj = new_hgt_dem;

3
libraries/AP_TECS/AP_TECS.h
View File

@ -290,6 +290,9 @@ private:
// counter for demanded sink rate on land final
uint8_t _flare_counter;
// slew height demand lag filter value when transition to land
float hgt_dem_lag_filter_slew;
// percent traveled along the previous and next waypoints
float _path_proportion;