dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #307 from PX4/pr-constrain_vpos 

constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance …
This commit is contained in:
Paul Riseborough 2017-07-20 17:35:35 +10:00 committed by GitHub
parent 9b6e1ab2a7 61a7991693
commit 627f8184b3
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/terrain_estimator.cpp
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);
}
}
}