fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 12:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mag_ctrl: combine common conditions for mag_hdg and mag_3d

Browse Source
This commit is contained in:
bresch
2024-05-17 11:19:04 +02:00
committed by Mathieu Bresciani
parent d796009302
commit ea39032b45
1 changed files with 16 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
+16 -21
View File
@@ -150,34 +150,29 @@ void Ekf::controlMagFusion()
&& (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset && (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset
&& isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL); && isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL);
checkMagHeadingConsistency(mag_sample); checkMagHeadingConsistency(mag_sample);
// WMM update can occur on the last epoch, just after mag fusion // WMM update can occur on the last epoch, just after mag fusion
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);
{
// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
// before they are used to constrain heading drift
_control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO)
&& _control_status.flags.mag
&& _control_status.flags.mag_aligned_in_flight
&& (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps)
&& !_control_status.flags.mag_fault
&& !_control_status.flags.ev_yaw
&& !_control_status.flags.gps_yaw;
const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps; const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
const bool common_conditions_passing = _control_status.flags.mag
&& ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
|| (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
&& !_control_status.flags.mag_fault
&& !_control_status.flags.mag_field_disturbed
&& !_control_status.flags.ev_yaw
&& !_control_status.flags.gps_yaw;
_control_status.flags.mag_hdg = ((_params.mag_fusion_type == MagFuseType::HEADING) _control_status.flags.mag_3D = common_conditions_passing
|| (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)) && (_params.mag_fusion_type == MagFuseType::AUTO)
&& _control_status.flags.mag && _control_status.flags.mag_aligned_in_flight;
&& ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
|| (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) _control_status.flags.mag_hdg = common_conditions_passing
&& !_control_status.flags.mag_fault && ((_params.mag_fusion_type == MagFuseType::HEADING)
&& !_control_status.flags.mag_field_disturbed || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
&& !_control_status.flags.ev_yaw }
&& !_control_status.flags.gps_yaw;
// TODO: allow clearing mag_fault if mag_3d is good? // TODO: allow clearing mag_fault if mag_3d is good?