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

EKF: Simplify terrain position validity check

Browse Source 
Remove the relative accuracy check that was false triggering close to ground
This commit is contained in:
devbharat
2017-02-22 14:12:05 +01:00
committed by Roman
parent af26ade1ce
commit d9688fae7d
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+5 -3
View File
@@ -135,10 +135,12 @@ bool Ekf::get_terrain_vert_pos(float *ret)
{
memcpy(ret, &_terrain_vpos, sizeof(float));
// The height is useful if the uncertainty in terrain height is significantly smaller than than the estimated height above terrain
bool accuracy_useful = (sqrtf(_terrain_var) < 0.2f * fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance));
if (_terrain_initialised && (_time_last_imu - _time_last_hagl_fuse < 1e6)) {
return true;
return (_time_last_imu - _time_last_hagl_fuse < 1e6) || accuracy_useful;
} else {
return false;
}
}
void Ekf::get_hagl_innov(float *hagl_innov)