diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 507a60b24e..91969e174d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -690,10 +690,7 @@ void NavEKF3_core::readGpsData() if (gpsGoodToAlign && !have_table_earth_field) { const auto *compass = dal.get_compass(); if (compass && compass->have_scale_factor(magSelectIndex) && compass->auto_declination_enabled()) { - table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc).toftype(); - table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7, - gpsloc.lng*1.0e-7)); - have_table_earth_field = true; + getEarthFieldTable(gpsloc); if (frontend->_mag_ef_limit > 0) { // initialise earth field from tables stateStruct.earth_magfield = table_earth_field_ga; @@ -1456,3 +1453,27 @@ void NavEKF3_core::SampleDragData(const imu_elements &imu) } #endif // EK3_FEATURE_DRAG_FUSION } + +/* + get the earth mag field + */ +void NavEKF3_core::getEarthFieldTable(const Location &loc) +{ + table_earth_field_ga = AP_Declination::get_earth_field_ga(loc).toftype(); + table_declination = radians(AP_Declination::get_declination(loc.lat*1.0e-7, + loc.lng*1.0e-7)); + have_table_earth_field = true; + last_field_update_ms = imuSampleTime_ms; +} + +/* + check if we should update the earth field, we update it at 1Hz + */ +void NavEKF3_core::checkUpdateEarthField(void) +{ + if (have_table_earth_field && imuSampleTime_ms - last_field_update_ms > 1000) { + Location loc = EKF_origin; + loc.offset(stateStruct.position.x, stateStruct.position.y); + getEarthFieldTable(loc); + } +} diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index a87a0a14f8..9185c84626 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -725,6 +725,9 @@ void NavEKF3_core::UpdateFilter(bool predict) statesInitialised = false; InitialiseFilterBootstrap(); } + + // check for update of earth field + checkUpdateEarthField(); } void NavEKF3_core::correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 704810558e..bf22be313b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1403,7 +1403,12 @@ private: bool have_table_earth_field; // true when we have initialised table_earth_field_ga Vector3F table_earth_field_ga; // earth field from WMM tables ftype table_declination; // declination in radians from the tables + uint32_t last_field_update_ms; + // handle earth field updates + void getEarthFieldTable(const Location &loc); + void checkUpdateEarthField(void); + // timing statistics struct ekf_timing timing;