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

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -548,6 +548,23 @@ void NavEKF3_core::readGpsData()
     // check for new GPS data
     const auto &gps = dal.gps();
 
+    Vector3f magNED;
+    getMagNED(magNED);
+    if (gps.status() == AP_DAL_GPS::NO_GPS && validOrigin) {
+          // if there is not gps, we should still align if we have a valid origin
+          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;
+              }
+          }
+      return;
+    }
+
     // limit update rate to avoid overflowing the FIFO buffer
     if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) {
         return;