diff --git a/EKF/control.cpp b/EKF/control.cpp index ef92c4d9a1..aa5ae33cad 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -616,6 +616,10 @@ void Ekf::controlGpsFusion() // Handle the case where we are using GPS and another source of aiding and GPS is failing checks if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) { _control_status.flags.gps = false; + // Reset position state to external vision if we are going to use absolute values + if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { + resetPosition(); + } ECL_WARN("EKF GPS data quality poor - stopping use"); }