From 641813758f1ce6ec26e7b8d209f9a94529aa039a Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Wed, 22 Nov 2023 13:59:02 -0300
Subject: [PATCH] EKF3: allow earth-frame fields to be estimated with an origin
 but no GPS

---
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index 2f858548721ca8..7a699246fe2ea7 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -682,6 +682,24 @@ 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;
+
+    // this section is similar to what is in AP_NavEKF3_Measurements.cpp's NavEKF3_core::readGpsData()
+    // but we do want to populate the WMM table even if we don't have a GPS at all
+    Vector3f magNED;
+    getMagNED(magNED);
+    if (!stateStruct.quat.is_zero()) {
+        alignMagStateDeclination();
+        const auto &compass = dal.compass();
+        if (compass.have_scale_factor(magSelectIndex) &&
+            compass.auto_declination_enabled()) {
+            getEarthFieldTable(EKF_origin);
+            if (frontend->_mag_ef_limit > 0) {
+                // initialise earth field from tables
+                stateStruct.earth_magfield = table_earth_field_ga;
+            }
+        }
+    }
+
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u origin set",(unsigned)imu_index);
 
     if (!frontend->common_origin_valid) {