Browse Source

Add option to warn the pilot in case of strong magnetic interference but still allow arming.

This PR changes the COM_ARM_MAG_STR parameter to accept values. If the parameter is set to 2, the check is performed and a warning is logged but the vehicle can still arm.
master
Michael Schaeuble 4 years ago committed by Mathieu Bresciani
parent
commit
5e1f62e9d0
  1. 23
      src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
  2. 7
      src/modules/commander/commander_params.c

23
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@ -49,8 +49,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & @@ -49,8 +49,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
{
bool success = true; // start with a pass and change to a fail if any test fails
int32_t mag_strength_check_enabled = 1;
param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled);
int32_t mag_strength_check = 1;
param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check);
float hgt_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit);
@ -107,13 +107,20 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & @@ -107,13 +107,20 @@ 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");
}
if ((mag_strength_check >= 1) && status.pre_flt_fail_mag_field_disturbed) {
const char *message = "Preflight%s: Strong magnetic interference detected";
success = false;
goto out;
if (mag_strength_check == 1) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, message, " Fail");
}
success = false;
goto out;
} else if (report_fail) {
mavlink_log_warning(mavlink_log_pub, message, "");
}
}
// check vertical position innovation test ratio

7
src/modules/commander/commander_params.c

@ -620,10 +620,13 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45); @@ -620,10 +620,13 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45);
/**
* Enable mag strength preflight check
*
* Deny arming if the estimator detects a strong magnetic
* Check if the estimator detects a strong magnetic
* disturbance (check enabled by EKF2_MAG_CHECK)
*
* @boolean
* @value 0 Disabled
* @value 1 Deny arming
* @value 2 Warning only
*
* @group Commander
*/
PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 1);

Loading…
Cancel
Save