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

Merge pull request #293 from PX4/pr-fixExtVisionFallback

Browse Source 
EKF: Allow fallback to non-aiding mode if external vision fusion is lost
This commit is contained in:
Paul Riseborough
2017-07-13 20:45:24 +09:00
committed by GitHub
parent 7a17f74910 9ee35e38df
commit d69cc3d7c8
1 changed files with 16 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+16 -1
View File
@@ -231,6 +231,21 @@ void Ekf::controlExternalVisionFusion()
fuseHeading(); fuseHeading();
} }
} }
// handle the case when we are relying on ev data and lose it
if (_control_status.flags.ev_pos && !_control_status.flags.gps && !_control_status.flags.opt_flow) {
// We are relying on ev aiding to constrain drift so after 5s without aiding we need to do something
if ((_time_last_imu - _time_last_pos_fuse > 5e6)) {
// Switch to the non-aiding mode, zero the velocity states
// and set the synthetic position to the current estimate
_control_status.flags.ev_pos = false;
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
}
}
} }
void Ekf::controlOpticalFlowFusion() void Ekf::controlOpticalFlowFusion()
@@ -321,7 +336,7 @@ void Ekf::controlOpticalFlowFusion()
} }
// handle the case when we are relying on optical flow fusion and lose it // handle the case when we are relying on optical flow fusion and lose it
if (_control_status.flags.opt_flow && !_control_status.flags.gps) { if (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos) {
// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something // We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
if ((_time_last_imu - _time_last_of_fuse > 5e6)) { if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
// Switch to the non-aiding mode, zero the velocity states // Switch to the non-aiding mode, zero the velocity states