diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 42d9f5d2e1..5dbf3f3339 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2206,29 +2206,31 @@ Commander::run() } /* Check for failure detector status */ - if (armed.armed && !_in_flight_termination) { + const bool failure_detector_updated = _failure_detector.update(); + if (failure_detector_updated) { - if (_failure_detector.update()) { + const uint8_t failure_status = _failure_detector.get_status(); - const uint8_t failure_status = _failure_detector.get_status(); + if (failure_status != status.failure_detector_status) { + status.failure_detector_status = failure_status; + status_changed = true; + } + } - if (failure_status != status.failure_detector_status) { - status.failure_detector_status = failure_status; - status_changed = true; - } + if (armed.armed && + failure_detector_updated && + !_in_flight_termination && + !status_flags.circuit_breaker_flight_termination_disabled) { - if (!status_flags.circuit_breaker_flight_termination_disabled) { - if (failure_status != 0) { + if (status.failure_detector_status != 0) { - armed.force_failsafe = true; - status_changed = true; + armed.force_failsafe = true; + status_changed = true; - _in_flight_termination = true; + _in_flight_termination = true; - mavlink_log_critical(&mavlink_log_pub, "Attitude failure detected: force failsafe"); - set_tune_override(TONE_PARACHUTE_RELEASE_TUNE); - } - } + mavlink_log_critical(&mavlink_log_pub, "Attitude failure detected: force failsafe"); + set_tune_override(TONE_PARACHUTE_RELEASE_TUNE); } } diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index e9fa2279e3..ae92457371 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -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; }