fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 03:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: remove legacy functions for terrain estimation

Browse Source
This commit is contained in:
Paul Riseborough
2022-02-22 15:27:27 +01:00
committed by bresch
parent d1fd00b92a
commit ea8465c277
7 changed files with 55 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+3 -3
View File
@@ -909,8 +909,8 @@ void Ekf::controlHeightFusion()
}
// Fuse range finder data if available
const bool use_for_hagl = _terrain_initialised && shouldUseRangeFinderForHagl();
if ((use_for_hagl || _control_status.flags.rng_hgt) && _range_sensor.isDataHealthy()) {
controlHaglFusion();
if ((_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder) && _range_sensor.isDataHealthy()) {
fuseHaglAllStates();
}
}
@@ -922,7 +922,7 @@ void Ekf::checkRangeAidSuitability()
&& isTerrainEstimateValid()) {
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
const float range_hagl = _terrain_vpos - _state.pos(2);
const float range_hagl = _state.posd_terrain - _state.pos(2);
const float range_hagl_max = _is_range_aid_suitable ? _params.max_hagl_for_range_aid : (_params.max_hagl_for_range_aid * 0.7f);
const bool is_in_range = range_hagl < range_hagl_max;