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

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -548,6 +548,26 @@ 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) {
+        printf("No GPS but valid origin\n");
+        // print origin
+        printf("Origin: %d %d %d\n", EKF_origin.lat, EKF_origin.lng, EKF_origin.alt);
+        // 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;