From 6f990ae96e5219a6c60c564aeb13e22adaa679cf Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 30 Oct 2023 22:03:53 -0400 Subject: [PATCH] AP_AHRS: prevent DCM fallback when losing GPS DCM fallback should only occur when GPS stops fusing, not when it is lost completely. --- libraries/AP_AHRS/AP_AHRS.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 4e061a465ec5d..d7d863f2ba66a 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -1930,11 +1930,11 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const should_use_gps = true; } #endif + const bool have_gps = AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D; if (hal.util->get_soft_armed() && (!filt_state.flags.using_gps || !filt_state.flags.horiz_pos_abs) && - should_use_gps && - AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) { + should_use_gps && have_gps) { // if the EKF is not fusing GPS or doesn't have a 2D fix // and we have a 3D lock, then plane and rover would // prefer to use the GPS position from DCM. This is a @@ -1942,7 +1942,7 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const // out return EKFType::DCM; } - if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) { + if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode && have_gps) { return EKFType::DCM; } if (!filt_state.flags.attitude ||