ekf2: mag 3d control HAGL reset wait until observable

2023-09-22 09:44:26 -04:00
1 changed files with 2 additions and 1 deletions
--- a/src/modules/ekf2/EKF/mag_3d_control.cpp
+++ b/src/modules/ekf2/EKF/mag_3d_control.cpp
@ -51,7 +51,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 	bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
 					     && _control_status.flags.tilt_align
 					     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
-					     && (wmm_updated || checkHaglYawResetReq() || isRecent(_time_last_mov_3d_mag_suitable, (uint64_t)3e6))
+					     && isRecent(_time_last_mov_3d_mag_suitable, (uint64_t)3e6)
 					     && mag_sample.mag.longerThan(0.f)
 					     && mag_sample.mag.isAllFinite();

@ -165,6 +165,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 			// activate fusion, reset mag states and initialize variance if first init or in flight reset
 			if (!_control_status.flags.yaw_align
 			    || wmm_updated
+			    || checkHaglYawResetReq()
 			    || !_mag_decl_cov_reset
 			    || !_state.mag_I.longerThan(0.f)
 			    || (P.slice<3, 3>(16, 16).diag().min() < sq(0.0001f)) // mag_I