diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index f9e3c645cf..1e377f2a84 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -88,38 +88,40 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* ---- MAG ---- */ 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; 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 */ - 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); + bool prime_found = false; - 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)) { - prime_found = true; - } + /* check all sensors individually, but fail only for mandatory ones */ + 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 { - if (required) { - failed = true; - mag_fail_reported = true; + int32_t device_id = -1; + + if (magnometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { + + 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 */ if (!prime_found) { if (reportFailures && !failed) {