diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp index ececde2d87..679215d296 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp @@ -288,6 +288,20 @@ void AP_NavEKF_Source::align_inactive_sources() } } visual_odom->align_position_to_ahrs(align_posxy, align_posz); + + // consider aligning yaw: + if ((getYawSource() == SourceYaw::COMPASS) || + (getYawSource() == SourceYaw::GPS) || + (getYawSource() == SourceYaw::GPS_COMPASS_FALLBACK) || + (getYawSource() == SourceYaw::GSF)) { + for (uint8_t i=0; irequest_align_yaw_to_ahrs(); + break; + } + } + } #endif }