diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp index 852d52d635..b7802e9009 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp @@ -213,6 +213,17 @@ bool AP_NavEKF_Source::haveVelZSource() const return false; } +// get yaw source +AP_NavEKF_Source::SourceYaw AP_NavEKF_Source::getYawSource() const +{ + // check for special case of disabled compasses + if ((_source_set[active_source_set].yaw == SourceYaw::COMPASS) && (AP::dal().compass().get_num_enabled() == 0)) { + return SourceYaw::NONE; + } + + return _source_set[active_source_set].yaw; +} + // align position of inactive sources to ahrs void AP_NavEKF_Source::align_inactive_sources() { diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.h b/libraries/AP_NavEKF/AP_NavEKF_Source.h index 83e0e35caf..dcaf9f1fcb 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_Source.h +++ b/libraries/AP_NavEKF/AP_NavEKF_Source.h @@ -71,7 +71,7 @@ public: bool haveVelZSource() const; // get yaw source - SourceYaw getYawSource() const { return _source_set[active_source_set].yaw; } + SourceYaw getYawSource() const; // align position of inactive sources to ahrs void align_inactive_sources();