yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Failure Detector - Add Failure Detector check to preflight checks

Browse Source
This commit is contained in:
bresch
2018-08-29 13:45:07 +02:00
committed by Beat Küng
parent c73875cbf8
commit 81bb7888de
2 changed files with 55 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
src/modules/commander/PreflightCheck.cpp
View File

@@ -674,6 +674,35 @@ out:
return success;
}
static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, bool report_fail, bool prearm)
{
bool success = true;
if (!prearm) {
// Ignore power check after arming.
return true;
} else {
if (status.failure_detector_status != 0) {
success = false;
if (report_fail) {
if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ROLL FAILURE DETECTED");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: PITCH FAILURE DETECTED");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ALTITUDE FAILURE DETECTED");
}
}
}
}
return success;
}
bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
const hrt_abstime &time_since_boot)
@@ -689,6 +718,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT);
const bool checkDynamic = !hil_enabled;
const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check);
const bool checkFailureDetector = true;
bool checkAirspeed = false;
@@ -944,6 +974,13 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
}
}
/* ---- Failure Detector ---- */
if (checkFailureDetector) {
if (!failureDetectorCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) {
failed = true;
}
}
/* Report status */
return !failed;
}