diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 2f858548721ca8..0ba289cd9b8bb6 100644 --- 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 index b6293f65609bbc..89e810cae6d962 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -692,15 +692,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 index 6c1934d6ca57fa..322ebc6907ab20 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -235,6 +235,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;