From 44200e96496002d7f072fa980386f4b486c9d42f Mon Sep 17 00:00:00 2001 From: Anna Dai Date: Tue, 18 Dec 2018 14:24:39 +0100 Subject: [PATCH] add GPS drop out case to GPS fusion logic EKF waits 10s after GPS signal is lost before setting GPS control status flag to false. As the position information given by the alternative position sources drifts from the last GPS position, the controller over corrects. With this update, the time horizon until GPS control flag set to false is reduced and only alternative position source is used for estimation. tested with optical flow as position souce log from as is https://review.px4.io/plot_app?log=d624af5e-dde4-40ab-ba5b-a693a49f5a36 log with update https://review.px4.io/plot_app?log=13ed6dc3-22dd-43f8-b898-4add41d60439 --- EKF/control.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index e04f66f3c9..2ede3541de 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -676,6 +676,11 @@ void Ekf::controlGpsFusion() } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { _control_status.flags.gps = false; ECL_WARN("EKF GPS data stopped"); + } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) { + // Handle the case where we are fusing another position source along GPS, + // stop waiting for GPS after 1 s of lost signal + _control_status.flags.gps = false; + ECL_WARN("EKF GPS data stopped, using only EV or OF"); } }