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

EKF: fix bug causing height offset when GPS use stops 

This bug causes the last vertical velocity observation to be continuously fused.
This commit is contained in:
Paul Riseborough 2018-05-10 13:14:09 +10:00
parent 6cadc92285
commit e8e9e34a73
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/vel_pos_fusion.cpp
View File

@ -183,7 +183,7 @@ void Ekf::fuseVelPosHeight()
innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align; innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align;
// record the successful velocity fusion event // record the successful velocity fusion event
if ((_fuse_hor_vel || _fuse_hor_vel_aux) && vel_check_pass) { if ((_fuse_hor_vel || _fuse_hor_vel_aux || _fuse_vert_vel) && vel_check_pass) {
_time_last_vel_fuse = _time_last_imu; _time_last_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_vel_NED = false; _innov_check_fail_status.flags.reject_vel_NED = false;
@ -191,7 +191,7 @@ void Ekf::fuseVelPosHeight()
_innov_check_fail_status.flags.reject_vel_NED = true; _innov_check_fail_status.flags.reject_vel_NED = true;
} }
_fuse_hor_vel = _fuse_hor_vel_aux = false; _fuse_hor_vel = _fuse_hor_vel_aux = _fuse_vert_vel = false;
// record the successful position fusion event // record the successful position fusion event
if (pos_check_pass && _fuse_pos) { if (pos_check_pass && _fuse_pos) {