From fa42bba3b5dd4259ca83dfd25f8c1e7aca3d9067 Mon Sep 17 00:00:00 2001
From: NoahWe <74419424+NoahWe@users.noreply.github.com>
Date: Wed, 5 Nov 2025 11:28:36 +0100
Subject: [PATCH] [ekf2] Changes to mag fusion (#3557)

* ekf2 fuse mag changes

* remove ekf2 mag fusion type
---
 conf/modules/ins_ekf2.xml            |  2 +-
 sw/airborne/modules/ins/ins_ekf2.cpp | 17 +++++++++++------
 sw/airborne/modules/ins/ins_ekf2.h   |  2 --
 3 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/conf/modules/ins_ekf2.xml b/conf/modules/ins_ekf2.xml
index 5240faf2da..de1a2a3a62 100644
--- a/conf/modules/ins_ekf2.xml
+++ b/conf/modules/ins_ekf2.xml
@@ -7,6 +7,7 @@
     </description>
     <define name="INS_EKF2_OPTITRACK" value="false" description="Easy configuration of full optitrack (position- and yaw fusion, height, init origin)"/>
     <define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS)" description="The sensor that are used for position fusion"/>
+    <define name="INS_EKF2_MAG_FUSION_MODE" value="MAG_FUSE_TYPE_AUTO" description="Default magnetometer fusion type"/>
     <define name="INS_EKF2_VDIST_SENSOR_TYPE" value="VDIST_SENSOR_BARO" description="Primary sensor used for vertical distance"/>
     <define name="INS_EKF2_GPS_CHECK_MASK" value="(MASK_GPS_NSATS | MASK_GPS_HACC | MASK_GPS_SACC)" description="GPS checks enabled before initialization of the global positioning"/>
     <define name="INS_EKF2_EVP_NOISE" value="0.1" description="External vision position noise [m]"/>
@@ -49,7 +50,6 @@
       <dl_settings name="ekf2">
         <dl_setting var="ekf2.qnh" min="500" step="0.1" max="1500" shortname="QNH"/>
         <dl_setting var="ekf2.temp" min="-50" step="0.1" max="80" shortname="temperature"/>
-        <dl_setting var="ekf2.mag_fusion_type" min="0" step="1" max="5" shortname="mag_fusion" values="AUTO|HEADING|3D|AUTOFW|INDOOR|NONE" module="modules/ins/ins_ekf2" handler="change_param"/>
         <dl_setting var="ekf2.fusion_mode" min="0" max="1" step="1" shortname="remove_gps_yaw" values="FALSE|TRUE" module="modules/ins/ins_ekf2" handler="remove_gps" type="bool"/>
       </dl_settings>
     </dl_settings>
diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
index 5c76a75be5..bb9c062980 100644
--- a/sw/airborne/modules/ins/ins_ekf2.cpp
+++ b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -59,6 +59,9 @@
 #ifndef INS_EKF2_FUSION_MODE
 #define INS_EKF2_FUSION_MODE (MASK_USE_EVPOS | MASK_USE_EVVEL | MASK_USE_EVYAW)
 #endif
+#ifndef INS_EKF2_MAG_FUSION_TYPE
+#define INS_EKF2_MAG_FUSION_TYPE MAG_FUSE_TYPE_INDOOR
+#endif
 #ifndef INS_EKF2_VDIST_SENSOR_TYPE
 #define INS_EKF2_VDIST_SENSOR_TYPE VDIST_SENSOR_EV
 #endif
@@ -70,6 +73,12 @@
 #endif
 PRINT_CONFIG_VAR(INS_EKF2_FUSION_MODE)
 
+/** The EKF2 magnetometer fusion type */
+#ifndef INS_EKF2_MAG_FUSION_TYPE
+#define INS_EKF2_MAG_FUSION_TYPE MAG_FUSE_TYPE_AUTO
+#endif
+PRINT_CONFIG_VAR(INS_EKF2_MAG_FUSION_TYPE)
+
 /** The EKF2 primary vertical distance sensor type */
 #ifndef INS_EKF2_VDIST_SENSOR_TYPE
 #define INS_EKF2_VDIST_SENSOR_TYPE VDIST_SENSOR_BARO
@@ -394,7 +403,7 @@ static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
   ekf.get_ekf_soln_status(&soln_status);
 
   uint16_t innov_test_status;
-  float mag, vel, pos, hgt, tas, hagl, flow, beta, mag_decl;
+  float mag, vel, pos, hgt, tas, hagl, flow, beta, mag_decl = 0.0f;
   uint8_t terrain_valid, dead_reckoning;
   ekf.get_innovation_test_status(innov_test_status, mag, vel, pos, hgt, tas, hagl, beta);
   //ekf.get_flow_innov(&flow);
@@ -542,6 +551,7 @@ void ins_ekf2_init(void)
   /* Get the ekf parameters */
   ekf_params = ekf.getParamHandle();
   ekf_params->fusion_mode = INS_EKF2_FUSION_MODE;
+  ekf_params->mag_fusion_type = INS_EKF2_MAG_FUSION_TYPE;
   ekf_params->vdist_sensor_type = INS_EKF2_VDIST_SENSOR_TYPE;
   ekf_params->gps_check_mask = INS_EKF2_GPS_CHECK_MASK;
 
@@ -776,11 +786,6 @@ void ins_ekf2_update(void)
   ekf2.got_imu_data = false;
 }
 
-void ins_ekf2_change_param(int32_t unk)
-{
-  ekf_params->mag_fusion_type = ekf2.mag_fusion_type = unk;
-}
-
 void ins_ekf2_remove_gps(int32_t mode)
 {
   if (mode) {
diff --git a/sw/airborne/modules/ins/ins_ekf2.h b/sw/airborne/modules/ins/ins_ekf2.h
index fec63df9e4..3f96da0786 100644
--- a/sw/airborne/modules/ins/ins_ekf2.h
+++ b/sw/airborne/modules/ins/ins_ekf2.h
@@ -55,13 +55,11 @@ struct ekf2_t {
   struct LtpDef_i ltp_def;        ///< Latest LTP definition from the quat_reset_counter EKF2
   bool got_imu_data;              ///< If we received valid IMU data (any sensor)
 
-  int32_t mag_fusion_type;
   int32_t fusion_mode;
 };
 
 extern void ins_ekf2_init(void);
 extern void ins_ekf2_update(void);
-extern void ins_ekf2_change_param(int32_t unk);
 extern void ins_ekf2_remove_gps(int32_t mode);
 extern void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *buf);
 extern void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t *buf);