Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: update earth field at 1Hz 

this prevents large mag errors on long distance flights
This commit is contained in:
Andrew Tridgell 2021-06-28 11:34:01 +10:00 committed by Randy Mackay
parent 399cd4ea03
commit e409202bef
3 changed files with 33 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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);
}
}

3
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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)

5
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1403,6 +1403,11 @@ 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;