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

EKF: enable separate monitoring of aux velocity innovations

Browse Source
This commit is contained in:
Paul Riseborough
2017-11-21 11:18:09 +11:00
parent 2a57fd858d
commit b0ad8269a5
5 changed files with 38 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+4 -4
View File
@@ -1368,10 +1368,10 @@ void Ekf::controlAuxVelFusion()
bool primary_aiding = _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow;
if (data_ready && primary_aiding) {
_fuse_vert_vel = _fuse_pos = _fuse_height = false;
_fuse_hor_vel = true;
_vel_pos_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
_vel_pos_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
_fuse_hor_vel_aux = true;
_aux_vel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
_aux_vel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
_velObsVarNE = _auxvel_sample_delayed.velVarNE;
_hvelInnovGate = _params.auxvel_gate;
fuseVelPosHeight();