EKF: fix bug causing height offset when GPS use stops

This bug causes the last vertical velocity observation to be continuously fused.
PX4 · May 10, 2018 · 75e6590 · 75e6590
1 parent b227aca
commit 75e6590
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp
@@ -183,15 +183,15 @@ void Ekf::fuseVelPosHeight()
 	innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align;
 
 	// 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;
 		_innov_check_fail_status.flags.reject_vel_NED = false;
 
 	} else if (!vel_check_pass) {
 		_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
 	if (pos_check_pass && _fuse_pos) {