AP_NavEKF3: log mag fusion selection to XKMF

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -4,6 +4,7 @@
 #include "AP_NavEKF3_core.h"
 
 #include <GCS_MAVLink/GCS.h>
+#include <AP_Logger/AP_Logger.h>
 #include <AP_DAL/AP_DAL.h>
 
 // minimum GPS horizontal speed required to use GPS ground course for yaw alignment (m/s)
@@ -425,12 +426,24 @@ 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) {
+#if HAL_LOGGING_ENABLED
+            if (magFusionSel != MagFuseSel::FUSE_YAW) {
+                magFusionSel = MagFuseSel::FUSE_YAW;
+                WriteMagFusionSelection();
+            }
+#endif
             fuseEulerYaw(yawFusionMethod::MAGNETOMETER);
 
             // zero the test ratio output from the inactive 3-axis magnetometer fusion
             magTestRatio.zero();
 
         } else {
+#if HAL_LOGGING_ENABLED
+            if (magFusionSel != MagFuseSel::FUSE_MAG) {
+                magFusionSel = MagFuseSel::FUSE_MAG;
+                WriteMagFusionSelection();
+            }
+#endif
             // 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 +475,20 @@ void NavEKF3_core::SelectMagFusion()
     }
 }
 
+#if HAL_LOGGING_ENABLED
+// log changes to magnetometer fusion selection
+void NavEKF3_core::WriteMagFusionSelection()
+{
+    const struct log_XKMF pkt{
+            LOG_PACKET_HEADER_INIT(LOG_XKMF_MSG),
+            time_us     : AP_HAL::micros64(),
+            core        : core_index,
+            fuse_sel    : static_cast<uint8_t>(magFusionSel)
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+#endif
+
 /*
  * 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:

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -446,6 +446,9 @@ void NavEKF3_core::InitialiseVariablesMag()
 #endif
     needMagBodyVarReset = false;
     needEarthBodyVarReset = false;
+#if HAL_LOGGING_ENABLED
+    magFusionSel = MagFuseSel::NOT_FUSING;
+#endif
 }
 
 /*

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -422,6 +422,15 @@ class NavEKF3_core : public NavEKF_core_common
         // 6 was EXTERNAL_YAW_FALLBACK (do not use)
     };
 
+#if HAL_LOGGING_ENABLED
+    // magnetometer fusion selections
+    enum class MagFuseSel {
+        NOT_FUSING = 0,
+        FUSE_YAW = 1,
+        FUSE_MAG = 2
+    };
+#endif
+
     // are we using (aka fusing) a non-compass yaw?
     bool using_noncompass_for_yaw(void) const;
 
@@ -800,6 +809,11 @@ class NavEKF3_core : public NavEKF_core_common
     // determine when to perform fusion of magnetometer measurements
     void SelectMagFusion();
 
+#if HAL_LOGGING_ENABLED
+    // log changes to magnetometer fusion selection
+    void WriteMagFusionSelection();
+#endif
+
     // determine when to perform fusion of true airspeed measurements
     void SelectTasFusion();
 
@@ -1418,6 +1432,9 @@ 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
+#if HAL_LOGGING_ENABLED
+    MagFuseSel magFusionSel;        // magnetometer fusion selection
+#endif
 
     // Used by on ground movement check required when operating on ground without a yaw reference
     ftype gyro_diff;                    // filtered gyro difference (rad/s)

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, \
@@ -373,6 +374,18 @@ struct PACKED log_XKTV {
     float tvd;
 };
 
+// @LoggerMessage: XKMF
+// @Description: EKF3 Magnetometer Fusion Selection
+// @Field: TimeUS: Time since system startup
+// @Field: C: EKF3 core this data is for
+// @Field: S: Magnetometer fusion selection: 0 not fusing, 1 fuse yaw, 2 fuse mag
+struct PACKED log_XKMF {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t core;
+    uint8_t fuse_sel;
+};
+
 // @LoggerMessage: XKV1
 // @Description: EKF3 State variances (primary core)
 // @Field: TimeUS: Time since system startup
@@ -449,6 +462,8 @@ struct PACKED log_XKV {
       "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, \
     { LOG_XKTV_MSG, sizeof(log_XKTV),                         \
       "XKTV", "QBff", "TimeUS,C,TVS,TVD", "s#rr", "F-00", true }, \
+    { LOG_XKMF_MSG, sizeof(log_XKMF), \
+      "XKMF", "QBB", "TimeUS,C,S", "s#-", "F--", true }, \
     { LOG_XKV1_MSG, sizeof(log_XKV), \
       "XKV1","QBffffffffffff","TimeUS,C,V00,V01,V02,V03,V04,V05,V06,V07,V08,V09,V10,V11", "s#------------", "F-------------" , true }, \
     { LOG_XKV2_MSG, sizeof(log_XKV), \