AP_NavEKF3: log mag fusion selection to XKMF

ArduPilot · Jul 8, 2024 · 0f8eb1c · 0f8eb1c
1 parent 38ca478
commit 0f8eb1c
Show file tree

Hide file tree

Showing 5 changed files with 18 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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 }, \