Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
1 changed files with 26 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

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