|
|
|
@ -471,7 +471,7 @@ void NavEKF3_core::SelectVelPosFusion()
@@ -471,7 +471,7 @@ void NavEKF3_core::SelectVelPosFusion()
|
|
|
|
|
} |
|
|
|
|
velPosObs[3] = gpsDataDelayed.pos.x; |
|
|
|
|
velPosObs[4] = gpsDataDelayed.pos.y; |
|
|
|
|
} else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV)) { |
|
|
|
|
} else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV)) { |
|
|
|
|
// use external nav system for horizontal position
|
|
|
|
|
extNavUsedForPos = true; |
|
|
|
|
fusePosData = true; |
|
|
|
@ -497,7 +497,7 @@ void NavEKF3_core::SelectVelPosFusion()
@@ -497,7 +497,7 @@ void NavEKF3_core::SelectVelPosFusion()
|
|
|
|
|
selectHeightForFusion(); |
|
|
|
|
|
|
|
|
|
// if we are using GPS, check for a change in receiver and reset position and height
|
|
|
|
|
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) && (gpsDataDelayed.sensor_idx != last_gps_idx || posxy_source_reset)) { |
|
|
|
|
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::GPS) && (gpsDataDelayed.sensor_idx != last_gps_idx || posxy_source_reset)) { |
|
|
|
|
// mark a source reset as consumed
|
|
|
|
|
posxy_source_reset = false; |
|
|
|
|
|
|
|
|
@ -514,7 +514,7 @@ void NavEKF3_core::SelectVelPosFusion()
@@ -514,7 +514,7 @@ void NavEKF3_core::SelectVelPosFusion()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// check for external nav position reset
|
|
|
|
|
if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV) && (extNavDataDelayed.posReset || posxy_source_reset)) { |
|
|
|
|
if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV) && (extNavDataDelayed.posReset || posxy_source_reset)) { |
|
|
|
|
// mark a source reset as consumed
|
|
|
|
|
posxy_source_reset = false; |
|
|
|
|
ResetPositionNE(extNavDataDelayed.pos.x, extNavDataDelayed.pos.y); |
|
|
|
|