Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Willian Galvani 2023-11-22 13:59:02 -03:00 committed by Andrew Tridgell
parent fb3335378c
commit cb74ebbddd
3 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -668,6 +668,20 @@ bool NavEKF3_core::setOriginLLH(const Location &loc)
return setOrigin(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) // sets the local NED origin using a LLH location (latitude, longitude, height)
// returns false is the origin has already been set // returns false is the origin has already been set
bool NavEKF3_core::setOrigin(const Location &loc) 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 // define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, EKF_origin.lat); calcEarthRateNED(earthRateNED, EKF_origin.lat);
validOrigin = true; 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); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u origin set",(unsigned)imu_index);
if (!frontend->common_origin_valid) { if (!frontend->common_origin_valid) {

10
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -692,15 +692,7 @@ void NavEKF3_core::readGpsData()
} }
if (gpsGoodToAlign && !have_table_earth_field) { if (gpsGoodToAlign && !have_table_earth_field) {
const auto &compass = dal.compass(); setEarthFieldFromLocation(gpsloc);
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;
}
}
} }
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -235,6 +235,9 @@ public:
// Returns true if the set was successful // Returns true if the set was successful
bool setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms); 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 estimated height above ground level
// return false if ground height is not being estimated. // return false if ground height is not being estimated.
bool getHAGL(float &HAGL) const; bool getHAGL(float &HAGL) const;