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

commander: skip all mag checks if SYS_HAS_MAG is 0

Browse Source
This commit is contained in:
Daniel Agar
2019-12-18 11:54:40 -08:00
parent a46add9a93
commit 7fd4db1881
1 changed files with 23 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
+23 -21
View File
@@ -88,38 +88,40 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- MAG ---- */ /* ---- MAG ---- */
if (checkSensors) { if (checkSensors) {
bool prime_found = false;
int32_t prime_id = -1;
param_get(param_find("CAL_MAG_PRIME"), &prime_id);
int32_t sys_has_mag = 1; int32_t sys_has_mag = 1;
param_get(param_find("SYS_HAS_MAG"), &sys_has_mag); param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
bool mag_fail_reported = false; if (sys_has_mag == 1) {
/* check all sensors individually, but fail only for mandatory ones */ bool prime_found = false;
for (unsigned i = 0; i < max_optional_mag_count; i++) {
const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
const bool report_fail = (reportFailures && !failed && !mag_fail_reported);
int32_t device_id = -1; int32_t prime_id = -1;
param_get(param_find("CAL_MAG_PRIME"), &prime_id);
if (magnometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { bool mag_fail_reported = false;
if ((prime_id > 0) && (device_id == prime_id)) { /* check all sensors individually, but fail only for mandatory ones */
prime_found = true; for (unsigned i = 0; i < max_optional_mag_count; i++) {
} const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
const bool report_fail = (reportFailures && !failed && !mag_fail_reported);
} else { int32_t device_id = -1;
if (required) {
failed = true; if (magnometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
mag_fail_reported = true;
if ((prime_id > 0) && (device_id == prime_id)) {
prime_found = true;
}
} else {
if (required) {
failed = true;
mag_fail_reported = true;
}
} }
} }
}
if (sys_has_mag == 1) {
/* check if the primary device is present */ /* check if the primary device is present */
if (!prime_found) { if (!prime_found) {
if (reportFailures && !failed) { if (reportFailures && !failed) {