From 69ca654194138507c3c382ddb2a0eba845c61112 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 19 May 2015 15:56:36 +1000 Subject: [PATCH] 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 --- libraries/AP_NavEKF/AP_NavEKF.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 8c11b20d91..9731545518 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -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