AP_NavEKF3: update earth field at 1Hz

this prevents large mag errors on long distance flights

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -690,10 +690,7 @@ void NavEKF3_core::readGpsData()
             if (gpsGoodToAlign && !have_table_earth_field) {
                 const auto *compass = dal.get_compass();
                 if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) {
-                    table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc).toftype();
+                    getEarthFieldTable(gpsloc);
-                    table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
-                                                                            gpsloc.lng*1.0e-7));
-                    have_table_earth_field = true;
                     if (frontend->_mag_ef_limit > 0) {
                         // initialise earth field from tables
                         stateStruct.earth_magfield = table_earth_field_ga;
@@ -1456,3 +1453,27 @@ void NavEKF3_core::SampleDragData(const imu_elements &imu)
     }
 #endif // EK3_FEATURE_DRAG_FUSION
 }
+
+/*
+  get the earth mag field
+ */
+void NavEKF3_core::getEarthFieldTable(const Location &loc)
+{
+    table_earth_field_ga = AP_Declination::get_earth_field_ga(loc).toftype();
+    table_declination = radians(AP_Declination::get_declination(loc.lat*1.0e-7,
+                                                                loc.lng*1.0e-7));
+    have_table_earth_field = true;
+    last_field_update_ms = imuSampleTime_ms;
+}
+
+/*
+  check if we should update the earth field, we update it at 1Hz
+ */
+void NavEKF3_core::checkUpdateEarthField(void)
+{
+    if (have_table_earth_field && imuSampleTime_ms - last_field_update_ms > 1000) {
+        Location loc = EKF_origin;
+        loc.offset(stateStruct.position.x, stateStruct.position.y);
+        getEarthFieldTable(loc);
+    }
+}

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -725,6 +725,9 @@ void NavEKF3_core::UpdateFilter(bool predict)
         statesInitialised = false;
         InitialiseFilterBootstrap();
     }
+
+    // check for update of earth field
+    checkUpdateEarthField();
 }
 
 void NavEKF3_core::correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index)

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1403,7 +1403,12 @@ private:
     bool have_table_earth_field;   // true when we have initialised table_earth_field_ga
     Vector3F table_earth_field_ga; // earth field from WMM tables
     ftype table_declination;       // declination in radians from the tables
+    uint32_t last_field_update_ms;
 
+    // handle earth field updates
+    void getEarthFieldTable(const Location &loc);
+    void checkUpdateEarthField(void);
+    
     // timing statistics
     struct ekf_timing timing;