AP_NavEKF2: Enable clean entry into GPS aiding in-flight

The setting of the EKF origin and the entry into GPS aiding mode have been separated to make the logic clear.
The order of operations has been changed to ensure that when a reset to GPS is performed, a valid GPS measurement is available in the buffer
Declaration of GPS availability is not made unless the GPS data has been entered into the buffer
This commit is contained in:
Paul Riseborough 2015-11-06 09:48:21 +11:00
parent f99dbc70f9
commit a8983d075f

View File

@ -432,34 +432,37 @@ void NavEKF2_core::readGpsData()
calcGpsGoodForFlight(); calcGpsGoodForFlight();
} }
// read latitutde and longitude from GPS and convert to local NE position relative to the stored origin // Read the GPS locaton in WGS-84 lat,long,height coordinates
// If we don't have an origin, then set it to the current GPS coordinates
const struct Location &gpsloc = _ahrs->get_gps().location(); const struct Location &gpsloc = _ahrs->get_gps().location();
if (validOrigin) {
gpsDataNew.pos = location_diff(EKF_origin, gpsloc); // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
} else if (gpsGoodToAlign) { if (gpsGoodToAlign && !validOrigin) {
// Set the NE origin to the current GPS position // Set the NE origin to the current GPS position if not previously set
setOrigin(); if (!validOrigin) {
// Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly setOrigin();
alignMagStateDeclination(); // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
// Set the height of the NED origin to height of baro height datum relative to GPS height datum' alignMagStateDeclination();
EKF_origin.alt = gpsloc.alt - baroDataNew.hgt; // Set the height of the NED origin to height of baro height datum relative to GPS height datum'
// We are by definition at the origin at the instant of alignment so set NE position to zero EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
gpsDataNew.pos.zero();
// If GPS useage isn't explicitly prohibited, we switch to absolute position mode
if (isAiding && frontend->_fusionModeGPS != 3) {
PV_AidingMode = AID_ABSOLUTE;
// Initialise EKF position and velocity states
ResetPosition();
ResetVelocity();
} }
} }
// save measurement to buffer to be fused later // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
StoreGPS(); if (validOrigin) {
gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
StoreGPS();
// declare GPS available for use
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();
}
// declare GPS available for use
gpsNotAvailable = false;
} else { } else {
// report GPS fix status // report GPS fix status
gpsCheckStatus.bad_fix = true; gpsCheckStatus.bad_fix = true;