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

ArduPilot · Jul 1, 2024 · 15484e0 · 15484e0
1 parent 868a650
commit 15484e0
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 9 deletions.
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -668,6 +668,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)
@@ -682,6 +696,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) {

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -690,15 +690,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

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -233,6 +233,9 @@ class NavEKF3_core : public NavEKF_core_common
     // 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;