From 2b2c1e2d788b29c522aa4721af3bfe829e2a7b38 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Tue, 13 Feb 2018 15:22:46 +0100 Subject: [PATCH] AP_NavEKF3: only use bcn EKF is the alignement as been completed --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) 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;