Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

ekf2: do not continuously use mag decl fusion when GNSS fusion is active

Browse Source 
This prevents over-constraining the heading from mag fusion. An
incorrect mag yaw rotation can be absorbed as a declination error.
This commit is contained in:
bresch
2024-03-28 18:18:52 +01:00
committed by Daniel Agar
parent 2f51db7284
commit dae246d7e2
2 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_3d_control.cpp
+4 -5
View File
@@ -90,11 +90,10 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
// if we are using 3-axis magnetometer fusion, but without external NE aiding,
// then the declination must be fused as an observation to prevent long term heading drift
// fusing declination when gps aiding is available is optional, but recommended to prevent
// problem if the vehicle is static for extended periods of time
// fusing declination when gps aiding is available is optional.
const bool mag_decl_user_selected = (_params.mag_declination_source & GeoDeclinationMask::FUSE_DECL);
const bool not_using_ne_aiding = !_control_status.flags.gps;
_control_status.flags.mag_dec = (_control_status.flags.mag && (not_using_ne_aiding || mag_decl_user_selected));
const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
_control_status.flags.mag_dec = (_control_status.flags.mag && ((not_using_ne_aiding || !_control_status.flags.mag_aligned_in_flight) || mag_decl_user_selected));
if (_control_status.flags.mag) {
aid_src.timestamp_sample = mag_sample.time_us;
@@ -120,7 +119,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
// The normal sequence is to fuse the magnetometer data first before fusing
// declination angle at a higher uncertainty to allow some learning of
// declination angle over time.
const bool update_all_states = _control_status.flags.mag_3D || (_control_status.flags.mag_hdg && (getYawVar() > sq(_params.mag_heading_noise / 2.f)));
const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
const bool update_tilt = _control_status.flags.mag_3D;
fuseMag(mag_sample.mag, aid_src, update_all_states, update_tilt);
src/modules/ekf2/ekf2_params.c
+1 -1
View File
@@ -484,7 +484,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f);
* @bit 2 use declination as an observation
* @reboot_required true
*/
PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 3);
/**
* Type of magnetometer fusion