diff --git a/src/modules/ekf2/EKF/mag_3d_control.cpp b/src/modules/ekf2/EKF/mag_3d_control.cpp index 8d182da7d9..79ed2b6c9d 100644 --- a/src/modules/ekf2/EKF/mag_3d_control.cpp +++ b/src/modules/ekf2/EKF/mag_3d_control.cpp @@ -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); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 396ba96515..ebeae0e8e1 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -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