fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 14:14:06 +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-17 13:21:11 -08:00
parent a46add9a93
commit 7fd4db1881
1 changed files with 23 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View File

@ -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) {