AP_NavEKF2: Remove duplicate transition into GPS aiding

This decision is made in one place in setAidingMode()
This commit is contained in:
Paul Riseborough 2016-05-21 11:16:03 +10:00 committed by Andrew Tridgell
parent 7201f7d2fd
commit d2694fe5dc

View File

@ -437,15 +437,8 @@ void NavEKF2_core::readGpsData()
gpsNotAvailable = false;
}
// Commence GPS aiding when able to
if (readyToUseGPS() && PV_AidingMode != AID_ABSOLUTE) {
PV_AidingMode = AID_ABSOLUTE;
// Initialise EKF position and velocity states to last GPS measurement
ResetPosition();
ResetVelocity();
}
frontend->logging.log_gps = true;
} else {
// report GPS fix status
gpsCheckStatus.bad_fix = true;