From 630d3fa8a7f3624469f3025b321e2724328f3b17 Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Thu, 21 Mar 2024 10:56:00 -0700 Subject: [PATCH] AP_NavEKF3: log mag fusion selection to XKFS --- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 3 ++- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 2 ++ libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 1 + libraries/AP_NavEKF3/AP_NavEKF3_core.h | 8 ++++++++ libraries/AP_NavEKF3/LogStructure.h | 4 +++- 5 files changed, 16 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 726b091b3e71b..9a93e80ab0909 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -102,7 +102,8 @@ void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const airspeed_index : getActiveAirspeed(), source_set : frontend->sources.getPosVelYawSourceSet(), gps_good_to_align : gpsGoodToAlign, - wait_for_gps_checks : waitingForGpsChecks + wait_for_gps_checks : waitingForGpsChecks, + mag_fusion: (uint8_t) magFusionSel }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 24861543bac86..390dfceb8705a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -425,12 +425,14 @@ void NavEKF3_core::SelectMagFusion() if (dataReady) { // use the simple method of declination to maintain heading if we cannot use the magnetic field states if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) { + magFusionSel = MagFuseSel::FUSE_YAW; fuseEulerYaw(yawFusionMethod::MAGNETOMETER); // zero the test ratio output from the inactive 3-axis magnetometer fusion magTestRatio.zero(); } else { + magFusionSel = MagFuseSel::FUSE_MAG; // if we are not doing aiding with earth relative observations (eg GPS) then the declination is // maintained by fusing declination as a synthesised observation // We also fuse declination if we are using the WMM tables diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 722b82914a47b..e4934a3d96189 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -452,6 +452,7 @@ void NavEKF3_core::InitialiseVariablesMag() #endif needMagBodyVarReset = false; needEarthBodyVarReset = false; + magFusionSel = MagFuseSel::NOT_FUSING; } /* diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index f2a8ee1aa4e18..29b7310119db8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -427,6 +427,13 @@ class NavEKF3_core : public NavEKF_core_common // 6 was EXTERNAL_YAW_FALLBACK (do not use) }; + // magnetometer fusion selections + enum class MagFuseSel { + NOT_FUSING = 0, + FUSE_YAW = 1, + FUSE_MAG = 2 + }; + // are we using (aka fusing) a non-compass yaw? bool using_noncompass_for_yaw(void) const; @@ -1426,6 +1433,7 @@ class NavEKF3_core : public NavEKF_core_common ftype posDownAtLastMagReset; // vertical position last time the mag states were reset (m) ftype yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad) QuaternionF quatAtLastMagReset; // quaternion states last time the mag states were reset + MagFuseSel magFusionSel; // magnetometer fusion selection // Used by on ground movement check required when operating on ground without a yaw reference ftype gyro_diff; // filtered gyro difference (rad/s) diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index 59ea98f9e42b5..a5629ea3f3f6e 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -346,6 +346,7 @@ struct PACKED log_XKQ { // @Field: SS: Source Set (primary=0/secondary=1/tertiary=2) // @Field: GPS_GTA: GPS good to align // @Field: GPS_CHK_WAIT: Waiting for GPS checks to pass +// @Field: MAG_FUSION: Magnetometer fusion (0=not fusing/1=fuse yaw/2=fuse mag) struct PACKED log_XKFS { LOG_PACKET_HEADER; uint64_t time_us; @@ -357,6 +358,7 @@ struct PACKED log_XKFS { uint8_t source_set; uint8_t gps_good_to_align; uint8_t wait_for_gps_checks; + uint8_t mag_fusion; }; // @LoggerMessage: XKTV @@ -443,7 +445,7 @@ struct PACKED log_XKV { { LOG_XKFM_MSG, sizeof(log_XKFM), \ "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s#-----", "F------", true }, \ { LOG_XKFS_MSG, sizeof(log_XKFS), \ - "XKFS","QBBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA,GPS_CHK_WAIT", "s#-------", "F--------" , true }, \ + "XKFS","QBBBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA,GPS_CHK_WAIT,MAG_FUSION", "s#--------", "F---------" , true }, \ { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" , true }, \ { LOG_XKT_MSG, sizeof(log_XKT), \ "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, \