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 @@
+
@@ -49,7 +50,6 @@
-
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);