From 0f8eb1c48cc672eb6566ab6e852c15d6f1a682dd 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 XKMF --- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 3 ++- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 3 +++ libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 1 + libraries/AP_NavEKF3/AP_NavEKF3_core.h | 8 ++++++++ libraries/AP_NavEKF3/LogStructure.h | 5 ++++- 5 files changed, 18 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 726b091b3e71b5..9a93e80ab0909a 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 24861543bac861..a5611b35bd88dc 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 @@ -462,6 +464,7 @@ void NavEKF3_core::SelectMagFusion() } } + /* * Fuse magnetometer measurements using explicit algebraic equations generated with Matlab symbolic toolbox. * The script file used to generate these and other equations in this filter can be found here: diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 008c9e2c01c7e5..0bb6e208f5df0d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -446,6 +446,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 b6b21be2a0b3b1..00385803364613 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -422,6 +422,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; @@ -1418,6 +1425,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 59ea98f9e42b5b..a6f7d8e1e4cffe 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -15,6 +15,7 @@ LOG_XKQ_MSG, \ LOG_XKT_MSG, \ LOG_XKTV_MSG, \ + LOG_XKMF_MSG, \ LOG_XKV1_MSG, \ LOG_XKV2_MSG, \ LOG_XKY0_MSG, \ @@ -346,6 +347,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 +359,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 +446,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 }, \