AP_NavEKF: Update declination when we know our location

This ensures that when we start using GPS, that the EKF will be using the correct declination for that location
If declination is not known it defaults to zero
This commit is contained in:
Paul Riseborough 2015-05-19 15:56:36 +10:00 committed by Andrew Tridgell
parent c1c5e3598a
commit 69ca654194
1 changed files with 2 additions and 0 deletions

View File

@ -4163,6 +4163,8 @@ void NavEKF::readGpsData()
} else if (goodToAlign){
// 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 - hgtMea;
// We are by definition at the origin at the instant of alignment so set NE position to zero