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

ekf2: separate mag and mag heading control logic (#21212)

Browse Source 
- split mag_3d into new standalone mag fusion and mag fusion allowed to update all states (full mag_3d)
 - new dedicated control logic for mag/mag_3d fusion and standalone mag heading fusion
 - if WMM available use for mag_I and mag_B init
 - mag states reset if external yaw reset (yaw estimator, GPS yaw, etc)
 - mag reset if declination changed (eliminate _mag_yaw_reset_req)
 - mag fusion (but not mag_hdg or mag_3d) can be active during gps_yaw or ev_yaw (if yaw aligned north)

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2023-08-04 10:39:16 -04:00
committed by GitHub
co-authored by bresch
parent 7db667558e
commit d75bb62a65
26 changed files with 1650 additions and 1397 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/zero_innovation_heading_update.cpp
+3 -4
View File
@@ -44,13 +44,12 @@ void Ekf::controlZeroInnovationHeadingUpdate()
|| _control_status.flags.ev_yaw || _control_status.flags.gps_yaw;
// fuse zero innovation at a limited rate if the yaw variance is too large
if (_control_status.flags.tilt_align
&& !yaw_aiding
if(!yaw_aiding
&& isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
// Use an observation variance larger than usual but small enough
// to constrain the yaw variance just below the threshold
const float obs_var = 0.25f;
const float obs_var = _control_status.flags.tilt_align ? 0.25f : 0.001f;
estimator_aid_source1d_s aid_src_status{};
aid_src_status.observation = getEulerYaw(_state.quat_nominal);
@@ -61,7 +60,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
if ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
if (!_control_status.flags.tilt_align || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
// The yaw variance is too large, fuse fake measurement
aid_src_status.fusion_enabled = true;
fuseYaw(aid_src_status, H_YAW);