fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 21:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-mag_auto: always use mag 3D after takeoff

Browse Source
This commit is contained in:
bresch
2024-02-27 12:33:43 -05:00
committed by Daniel Agar
parent 086656dc7f
commit b80f15f7b5
10 changed files with 8 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_3d_control.cpp
+2 -5
View File
@@ -45,13 +45,12 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
resetEstimatorAidStatus(aid_src);
const bool wmm_updated = (_wmm_gps_time_last_set > aid_src.time_last_fuse);
const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse); // WMM update can occur on the last epoch, just after mag fusion
// determine if we should use mag fusion
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))
&& mag_sample.mag.longerThan(0.f)
&& mag_sample.mag.isAllFinite();
@@ -65,8 +64,6 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
&& _control_status.flags.mag_aligned_in_flight
&& (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps)
&& !_control_status.flags.mag_fault
&& isRecent(aid_src.time_last_fuse, 500'000)
&& getMagBiasVariance().longerThan(0.f) && !getMagBiasVariance().longerThan(sq(0.02f))
&& !_control_status.flags.ev_yaw
&& !_control_status.flags.gps_yaw;
@@ -92,7 +89,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
if (continuing_conditions_passing && _control_status.flags.yaw_align) {
if (mag_sample.reset || checkHaglYawResetReq()) {
if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) {
ECL_INFO("reset to %s", AID_SRC_NAME);
resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
aid_src.time_last_fuse = _time_delayed_us;