From 55f3dd7c118a3fb94e70f6afa51c015f1b129795 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 | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index d2179f348cbeda..0d1a4fc6e288bc 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -659,6 +659,22 @@ 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;
+
+    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) {