fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 06:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)

Browse Source
This commit is contained in:
ChristophTobler
2017-07-20 08:29:11 +02:00
parent 9b6e1ab2a7
commit 61a7991693
1 changed files with 5 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+5
View File
@@ -102,6 +102,11 @@ void Ekf::runTerrainEstimator()
_cos_tilt_rng = cosf(_params.rng_sens_pitch);
}
//constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
}
}
}