diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 551020923d..d49eed7705 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -1904,6 +1904,11 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const if (ret != EKFType::NONE && (_vehicle_class == VehicleClass::FIXED_WING || _vehicle_class == VehicleClass::GROUND)) { + if (!dcm.yaw_source_available() && !fly_forward) { + // if we don't have a DCM yaw source available and we are + // in a non-fly-forward mode then we are best off using the EKF + return ret; + } bool should_use_gps = true; nav_filter_status filt_state; #if HAL_NAVEKF2_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 5085dcd62a..b5c90eb2d2 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1254,3 +1254,9 @@ bool AP_AHRS_DCM::get_relative_position_D_origin(float &posD) const void AP_AHRS_DCM::send_ekf_status_report(GCS_MAVLINK &link) const { } + +// return true if DCM has a yaw source available +bool AP_AHRS_DCM::yaw_source_available(void) const +{ + return AP::compass().use_for_yaw(); +} diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index d5da18aa84..c3a5bd02c8 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -124,6 +124,9 @@ public: void send_ekf_status_report(class GCS_MAVLINK &link) const override; + // return true if DCM has a yaw source + bool yaw_source_available(void) const; + private: // settable parameters