diff --git a/EKF/control.cpp b/EKF/control.cpp index ec70953bb4..f8cfa69344 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1358,7 +1358,6 @@ void Ekf::controlVelPosFusion() // Fuse available NED velocity and position data into the main filter if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { fuseVelPosHeight(); - _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; } } @@ -1376,6 +1375,5 @@ void Ekf::controlAuxVelFusion() _velObsVarNE = _auxvel_sample_delayed.velVarNE; _hvelInnovGate = _params.auxvel_gate; fuseVelPosHeight(); - _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; } } diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index c1b2ccff94..83c13a2a2a 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -176,6 +176,7 @@ void Ekf::fuseVelPosHeight() } else if (!vel_check_pass) { _innov_check_fail_status.flags.reject_vel_NED = true; } + _fuse_hor_vel = false; // record the successful position fusion event if (pos_check_pass && _fuse_pos) { @@ -188,6 +189,7 @@ void Ekf::fuseVelPosHeight() } else if (!pos_check_pass) { _innov_check_fail_status.flags.reject_pos_NE = true; } + _fuse_pos = false; // record the successful height fusion event if (innov_check_pass_map[5] && _fuse_height) { @@ -196,6 +198,7 @@ void Ekf::fuseVelPosHeight() } else if (!innov_check_pass_map[5]) { _innov_check_fail_status.flags.reject_pos_D = true; } + _fuse_height = false; for (unsigned obs_index = 0; obs_index < 6; obs_index++) { // skip fusion if not requested or checks have failed