From 1bb576c19788b31490a6d7e51f1760f2a58e1706 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 4 Jul 2017 20:32:45 +1000 Subject: [PATCH 1/2] EKF: Allow fallback to non-aiding mode if external vision is lost. --- EKF/control.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index 70503338c3..55b3d12cf9 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -231,6 +231,21 @@ void Ekf::controlExternalVisionFusion() 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() From 9ee35e38df8884c261381b53d0a35d2ea55ceebc Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 4 Jul 2017 20:33:14 +1000 Subject: [PATCH 2/2] EKF: Don't fallback when optical flow is lost if external vision data is being used --- EKF/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 55b3d12cf9..97d3828085 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -336,7 +336,7 @@ void Ekf::controlOpticalFlowFusion() } // 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 if ((_time_last_imu - _time_last_of_fuse > 5e6)) { // Switch to the non-aiding mode, zero the velocity states