EKF3: allow earth-frame fields to be estimated with an origin but no GPS

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -645,6 +645,20 @@ bool NavEKF3_core::setOriginLLH(const Location &loc)
     return setOrigin(loc);
 }
 
+// populates the Earth magnetic field table using the given location
+void NavEKF3_core::setEarthFieldFromLocation(const Location &loc)
+{
+    const auto &compass = dal.compass();
+    if (compass.have_scale_factor(magSelectIndex) &&
+        compass.auto_declination_enabled()) {
+        getEarthFieldTable(loc);
+        if (frontend->_mag_ef_limit > 0) {
+            // initialise earth field from tables
+            stateStruct.earth_magfield = table_earth_field_ga;
+        }
+    }
+}
+
 // sets the local NED origin using a LLH location (latitude, longitude, height)
 // returns false is the origin has already been set
 bool NavEKF3_core::setOrigin(const Location &loc)
@@ -659,6 +673,13 @@ bool NavEKF3_core::setOrigin(const Location &loc)
     // define Earth rotation vector in the NED navigation frame at the origin
     calcEarthRateNED(earthRateNED, EKF_origin.lat);
     validOrigin = true;
+
+    // but we do want to populate the WMM table even if we don't have a GPS at all
+    if (!stateStruct.quat.is_zero()) {
+        alignMagStateDeclination();
+        setEarthFieldFromLocation(EKF_origin);
+    }
+
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u origin set",(unsigned)imu_index);
 
     if (!frontend->common_origin_valid) {

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -675,15 +675,7 @@ void NavEKF3_core::readGpsData()
     }
 
     if (gpsGoodToAlign && !have_table_earth_field) {
-        const auto &compass = dal.compass();
-        if (compass.have_scale_factor(magSelectIndex) &&
-            compass.auto_declination_enabled()) {
-            getEarthFieldTable(gpsloc);
-            if (frontend->_mag_ef_limit > 0) {
-                // initialise earth field from tables
-                stateStruct.earth_magfield = table_earth_field_ga;
-            }
-        }
+        setEarthFieldFromLocation(gpsloc);
     }
 
     // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -236,6 +236,9 @@ public:
     // Returns true if the set was successful
     bool setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms);
 
+    // Popoluates the WMM data structure with the field at the given location
+    void setEarthFieldFromLocation(const Location &loc);
+
     // return estimated height above ground level
     // return false if ground height is not being estimated.
     bool getHAGL(float &HAGL) const;