diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 0441b23681..f2f02a3069 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -447,7 +447,7 @@ bool NavEKF3_core::readyToUseGPS(void) const // return true if the filter to be ready to use the beacon range measurements bool NavEKF3_core::readyToUseRangeBeacon(void) const { - return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnGoodToAlign && rngBcnDataToFuse; + return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnAlignmentCompleted && rngBcnDataToFuse; } // return true if we should use the compass diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index d2aec11979..0823c7543d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -23,8 +23,8 @@ void NavEKF3_core::SelectRngBcnFusion() // Determine if we need to fuse range beacon data on this time step if (rngBcnDataToFuse) { if (PV_AidingMode == AID_ABSOLUTE) { - if (!filterStatus.flags.using_gps) { - if (!bcnOriginEstInit && rngBcnAlignmentCompleted) { + if (!filterStatus.flags.using_gps && rngBcnAlignmentCompleted) { + if (!bcnOriginEstInit) { bcnOriginEstInit = true; bcnPosOffsetNED.x = receiverPos.x - stateStruct.position.x; bcnPosOffsetNED.y = receiverPos.y - stateStruct.position.y;