From 226169192a24424ae01600afa9c7e375b4c36418 Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Sat, 28 Dec 2019 20:06:14 +0100 Subject: [PATCH] ekf2check: add COM_ARM_MAG_STR to en/disable mag field strength preflight check --- .../Arming/PreFlightCheck/checks/ekf2Check.cpp | 18 +++++++++++++----- src/modules/commander/commander_params.c | 11 +++++++++++ 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index 4064b6e410..bfb83a9579 100644 --- 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,9 +81,6 @@ 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"); } } @@ -89,6 +88,15 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & 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 index 1e0c55c4e0..15a7fd695e 100644 --- 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 *