Browse Source

EKF: clean up reset of fusion flags

master
Paul Riseborough 7 years ago
parent
commit
2a57fd858d
  1. 2
      EKF/control.cpp
  2. 3
      EKF/vel_pos_fusion.cpp

2
EKF/control.cpp

@ -1358,7 +1358,6 @@ void Ekf::controlVelPosFusion() @@ -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() @@ -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;
}
}

3
EKF/vel_pos_fusion.cpp

@ -176,6 +176,7 @@ void Ekf::fuseVelPosHeight() @@ -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() @@ -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() @@ -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

Loading…
Cancel
Save