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

ekf2-mag: do not reset on WMM change when NE aiding is active

Browse Source 
The mag field states are observed. No need to reset them.
This commit is contained in:
bresch
2024-05-31 10:38:17 -04:00
committed by Daniel Agar
parent ee765e7859
commit 3dac9af09e
1 changed files with 4 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
+4 -5
View File
@@ -180,15 +180,14 @@ void Ekf::controlMagFusion()
// if we are using 3-axis magnetometer fusion, but without external NE aiding,
// then the declination must be fused as an observation to prevent long term heading drift
// fusing declination when gps aiding is available is optional.
_control_status.flags.mag_dec = _control_status.flags.mag
&& (!using_ne_aiding || !_control_status.flags.mag_aligned_in_flight);
const bool no_ne_aiding_or_pre_takeoff = !using_ne_aiding || !_control_status.flags.mag_aligned_in_flight;
_control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_pre_takeoff;
if (_control_status.flags.mag) {
if (continuing_conditions_passing && _control_status.flags.yaw_align) {
if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) {
if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) {
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;
@@ -216,7 +215,7 @@ void Ekf::controlMagFusion()
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
if (is_fusion_failing) {
if (!using_ne_aiding || !_control_status.flags.mag_aligned_in_flight) {
if (no_ne_aiding_or_pre_takeoff) {
ECL_WARN("%s fusion failing, resetting", 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;