Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: only use bcn EKF is the alignement as been completed

Browse Source
This commit is contained in:
Pierre Kancir 2018-02-13 15:22:46 +01:00 committed by Randy Mackay
parent 44a21bc8ac
commit 2b2c1e2d78
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -447,7 +447,7 @@ bool NavEKF3_core::readyToUseGPS(void) const
// return true if the filter to be ready to use the beacon range measurements // return true if the filter to be ready to use the beacon range measurements
bool NavEKF3_core::readyToUseRangeBeacon(void) const 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 // return true if we should use the compass

4
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -23,8 +23,8 @@ void NavEKF3_core::SelectRngBcnFusion()
// Determine if we need to fuse range beacon data on this time step // Determine if we need to fuse range beacon data on this time step
if (rngBcnDataToFuse) { if (rngBcnDataToFuse) {
if (PV_AidingMode == AID_ABSOLUTE) { if (PV_AidingMode == AID_ABSOLUTE) {
if (!filterStatus.flags.using_gps) { if (!filterStatus.flags.using_gps && rngBcnAlignmentCompleted) {
if (!bcnOriginEstInit && rngBcnAlignmentCompleted) { if (!bcnOriginEstInit) {
bcnOriginEstInit = true; bcnOriginEstInit = true;
bcnPosOffsetNED.x = receiverPos.x - stateStruct.position.x; bcnPosOffsetNED.x = receiverPos.x - stateStruct.position.x;
bcnPosOffsetNED.y = receiverPos.y - stateStruct.position.y; bcnPosOffsetNED.y = receiverPos.y - stateStruct.position.y;