fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 02:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2check: add COM_ARM_MAG_STR to en/disable mag field strength preflight check

Browse Source
This commit is contained in:
Mathieu Bresciani
2019-12-28 14:06:14 -05:00
committed by Daniel Agar
parent 7bb952baed
commit 226169192a
2 changed files with 24 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
+13 -5
View File
@@ -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);
src/modules/commander/commander_params.c
+11
View File
@@ -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
*