From 29f0561ce455e06c9c59e21a57477b0da32c443d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 May 2015 18:49:01 +1000 Subject: [PATCH] AP_AHRS: reject EKF for plane when we have GPS and aren't fusing plane users would prefer to use GPS in this case --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index eb95168069..ecd682109e 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -324,6 +324,13 @@ bool AP_AHRS_NavEKF::using_EKF(void) const #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_APMrover2) nav_filter_status filt_state; EKF.getFilterStatus(filt_state); + if (hal.util->get_soft_armed() && !filt_state.flags.using_gps && _gps.status() >= AP_GPS::GPS_OK_FIX_3D) { + // if the EKF is not fusing GPS and we have a 3D lock, then + // plane and rover would prefer to use the GPS position from + // DCM. This is a safety net while some issues with the EKF + // get sorted out + return false; + } if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) { return false; }