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

AP_NavEKF3: allow alt from other sources when using ExtNav

Browse Source 
also allows using ExtNav alt when not using it for XY position
removes redundant and incorrect setting of activeHgtSource from ExtNav
which was always being overwritten by the correct value later in selectHeightForFusion
This commit is contained in:
Randy Mackay 2020-07-04 12:41:29 +09:00
parent 0cebde0632
commit 6989a28bfc
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -434,10 +434,8 @@ void NavEKF3_core::SelectVelPosFusion()
velPosObs[3] = gpsDataDelayed.pos.x;
velPosObs[4] = gpsDataDelayed.pos.y;
} else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS == 3)) {
// use external nav system for position
// use external nav system for horizontal position
extNavUsedForPos = true;
activeHgtSource = HGT_SOURCE_EXTNAV;
fuseHgtData = true;
fusePosData = true;
// correct for external navigation sensor position
@ -445,7 +443,6 @@ void NavEKF3_core::SelectVelPosFusion()
velPosObs[3] = extNavDataDelayed.pos.x;
velPosObs[4] = extNavDataDelayed.pos.y;
velPosObs[5] = extNavDataDelayed.pos.z;
}
// fuse external navigation velocity data if available
@ -1046,6 +1043,8 @@ void NavEKF3_core::selectHeightForFusion()
activeHgtSource = HGT_SOURCE_GPS;
} else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) {
activeHgtSource = HGT_SOURCE_BCN;
} else if ((frontend->_altSource == 4) && ((imuSampleTime_ms - extNavMeasTime_ms) < 500)) {
activeHgtSource = HGT_SOURCE_EXTNAV;
}
// Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon
@ -1086,7 +1085,9 @@ void NavEKF3_core::selectHeightForFusion()
// Select the height measurement source
if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
hgtMea = -extNavDataDelayed.pos.z;
velPosObs[5] = -hgtMea;
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
fuseHgtData = true;
} else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
// using range finder data
// correct for tilt using a flat earth model