Browse Source

AP_NavEKF3: use posxy_source in all places

c415-sdk
chobitsfan 4 years ago committed by Randy Mackay
parent
commit
26a3a02c81
  1. 6
      libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

6
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@ -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);

Loading…
Cancel
Save