EKF: Add function to calculate the magnetic declination to use

EKF/gps_checks.cpp

@@ -40,6 +40,7 @@
  */
 
 #include "ekf.h"
+#include <mathlib/mathlib.h>
 
 // GPS pre-flight check bit locations
 #define MASK_GPS_NSATS  (1<<0)
@@ -67,6 +68,8 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 			_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
 			_NED_origin_initialised = true;
 			_last_gps_origin_time_us = _time_last_imu;
+			// set the magnetic declination returned by the geo library using the current GPS position
+			_mag_declination_gps = math::radians(get_mag_declination(lat, lon));
 		}
 	}