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

AP_NavEKF3: beacon fusion checks source

Browse Source
This commit is contained in:
Randy Mackay 2020-08-17 21:03:34 +09:00
parent 8a56ce49be
commit 1263648d88
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
View File

@ -14,16 +14,16 @@ 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 && rngBcnAlignmentCompleted) { if ((frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::BEACON) && rngBcnAlignmentCompleted) {
if (!bcnOriginEstInit) { 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;
} }
// If we aren't using GPS, then the beacons are used as the primary means of position reference // beacons are used as the primary means of position reference
FuseRngBcn(); FuseRngBcn();
} else { } else {
// If we are using GPS, then GPS is the primary reference, but we continue to use the beacon data // If another source (i.e. GPS, ExtNav) is the primary reference, we continue to use the beacon data
// to calculate an independent position that is used to update the beacon position offset if we need to // to calculate an independent position that is used to update the beacon position offset if we need to
// start using beacon data as the primary reference. // start using beacon data as the primary reference.
FuseRngBcnStatic(); FuseRngBcnStatic();