ekf2check: add COM_ARM_MAG_STR to en/disable mag field strength prefl…

…ight check
PX4 · Dec 28, 2019 · 2261691 · 2261691
1 parent 7bb952b
commit 2261691
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 5 deletions.
diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
@@ -48,6 +48,9 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	bool present = true;
 	float test_limit = 1.0f; // pass limit re-used for each test
 
+	int32_t mag_strength_check_enabled = 1;
+	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled);
+
 	bool gps_success = true;
 	bool gps_present = true;
 
@@ -65,8 +68,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	if (status.pre_flt_fail_innov_heading ||
 	    status.pre_flt_fail_innov_vel_horiz ||
 	    status.pre_flt_fail_innov_vel_vert ||
-	    status.pre_flt_fail_innov_height ||
-	    status.pre_flt_fail_mag_field_disturbed) {
+	    status.pre_flt_fail_innov_height) {
 		if (report_fail) {
 			if (status.pre_flt_fail_innov_heading) {
 				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable");
@@ -79,16 +81,22 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 
 			} else if (status.pre_flt_fail_innov_height) {
 				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable");
-
-			} else if (status.pre_flt_fail_mag_field_disturbed) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: strong magnetic interference detected");
 			}
 		}
 
 		success = false;
 		goto out;
 	}
 
+	if ((mag_strength_check_enabled == 1) && status.pre_flt_fail_mag_field_disturbed) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: strong magnetic interference detected");
+		}
+
+		success = false;
+		goto out;
+	}
+
 	// check vertical position innovation test ratio
 	param_get(param_find("COM_ARM_EKF_HGT"), &test_limit);
 

diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
@@ -622,6 +622,17 @@ PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.25f);
  */
 PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 30);
 
+/**
+ * Enable mag strength preflight check
+ *
+ * Deny arming if the estimator detects a strong magnetic
+ * disturbance (check enabled by EKF2_MAG_CHECK)
+ *
+ * @boolean
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 1);
+
 /**
  * Enable RC stick override of auto modes
  *