dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: mag 3d control HAGL reset wait until observable

This commit is contained in:
Daniel Agar 2023-09-22 09:43:30 -04:00
parent db97a38a9d
commit 1b1004daf2
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf2/EKF/mag_3d_control.cpp
View File

@ -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) bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
&& _control_status.flags.tilt_align && _control_status.flags.tilt_align
&& (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_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.longerThan(0.f)
&& mag_sample.mag.isAllFinite(); && 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 // activate fusion, reset mag states and initialize variance if first init or in flight reset
if (!_control_status.flags.yaw_align if (!_control_status.flags.yaw_align
|| wmm_updated || wmm_updated
|| checkHaglYawResetReq()
|| !_mag_decl_cov_reset || !_mag_decl_cov_reset
|| !_state.mag_I.longerThan(0.f) || !_state.mag_I.longerThan(0.f)
|| (P.slice<3, 3>(16, 16).diag().min() < sq(0.0001f)) // mag_I || (P.slice<3, 3>(16, 16).diag().min() < sq(0.0001f)) // mag_I