|
|
|
@ -345,7 +345,7 @@ void NavEKF2_core::SelectVelPosFusion()
@@ -345,7 +345,7 @@ void NavEKF2_core::SelectVelPosFusion()
|
|
|
|
|
} else if (extNavDataToFuse && PV_AidingMode == AID_ABSOLUTE) { |
|
|
|
|
// This is a special case that uses and external nav system for position
|
|
|
|
|
extNavUsedForPos = true; |
|
|
|
|
activeHgtSource = HGT_SOURCE_EV; |
|
|
|
|
activeHgtSource = HGT_SOURCE_EXTNAV; |
|
|
|
|
fuseVelData = false; |
|
|
|
|
fuseHgtData = true; |
|
|
|
|
fusePosData = true; |
|
|
|
@ -875,7 +875,7 @@ void NavEKF2_core::selectHeightForFusion()
@@ -875,7 +875,7 @@ void NavEKF2_core::selectHeightForFusion()
|
|
|
|
|
// select height source
|
|
|
|
|
if (extNavUsedForPos) { |
|
|
|
|
// always use external vision as the height source if using for position.
|
|
|
|
|
activeHgtSource = HGT_SOURCE_EV; |
|
|
|
|
activeHgtSource = HGT_SOURCE_EXTNAV; |
|
|
|
|
} else if (_rng && ((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { |
|
|
|
|
if (frontend->_altSource == 1) { |
|
|
|
|
// always use range finder
|
|
|
|
@ -922,7 +922,7 @@ void NavEKF2_core::selectHeightForFusion()
@@ -922,7 +922,7 @@ void NavEKF2_core::selectHeightForFusion()
|
|
|
|
|
// Use Baro alt as a fallback if we lose range finder, GPS or external nav
|
|
|
|
|
bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && ((imuSampleTime_ms - rngValidMeaTime_ms) > 500)); |
|
|
|
|
bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000)); |
|
|
|
|
bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000)); |
|
|
|
|
bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EXTNAV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000)); |
|
|
|
|
if (lostRngHgt || lostGpsHgt || lostExtNavHgt) { |
|
|
|
|
activeHgtSource = HGT_SOURCE_BARO; |
|
|
|
|
} |
|
|
|
@ -954,7 +954,7 @@ void NavEKF2_core::selectHeightForFusion()
@@ -954,7 +954,7 @@ void NavEKF2_core::selectHeightForFusion()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Select the height measurement source
|
|
|
|
|
if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EV)) { |
|
|
|
|
if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) { |
|
|
|
|
hgtMea = -extNavDataDelayed.pos.z; |
|
|
|
|
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.01f, 10.0f)); |
|
|
|
|
} else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) { |
|
|
|
|