diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 21cae9bcb27936..567789c4e80c65 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -3047,6 +3047,31 @@ bool AP_AHRS::using_noncompass_for_yaw(void) const return false; } +// check if external nav is providing yaw +bool AP_AHRS::using_extnav_for_yaw(void) const +{ + switch (active_EKF_type()) { +#if HAL_NAVEKF2_AVAILABLE + case EKFType::TWO: + return EKF2.isExtNavUsedForYaw(); +#endif + case EKFType::NONE: +#if HAL_NAVEKF3_AVAILABLE + case EKFType::THREE: + return EKF3.using_extnav_for_yaw(); +#endif +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case EKFType::SIM: +#endif +#if HAL_EXTERNAL_AHRS_ENABLED + case EKFType::EXTERNAL: +#endif + return false; + } + // since there is no default case above, this is unreachable + return false; +} + // set and save the alt noise parameter value void AP_AHRS::set_alt_measurement_noise(float noise) { diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 886b8a542b4d12..e9d1dfca50a8a4 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -329,6 +329,9 @@ class AP_AHRS : public AP_AHRS_DCM { // check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed bool using_noncompass_for_yaw(void) const override; + // check if external nav is providing yaw + bool using_extnav_for_yaw(void) const override; + // set and save the ALT_M_NSE parameter value void set_alt_measurement_noise(float noise) override; diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 98d5f88bff356e..8ea582b0fd87c3 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -92,7 +92,10 @@ class AP_AHRS_Backend // check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed virtual bool using_noncompass_for_yaw(void) const { return false; } - + + // check if external nav is providing yaw + virtual bool using_extnav_for_yaw(void) const { return false; } + // request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe virtual void request_yaw_reset(void) {}