Failure Detector - Add Failure Detector check to preflight checks

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) {
+		if (armed.armed &&
-					status.failure_detector_status = failure_status;
+			failure_detector_updated &&
-					status_changed = true;
+			!_in_flight_termination &&
-				}
+			!status_flags.circuit_breaker_flight_termination_disabled) {
 
-				if (!status_flags.circuit_breaker_flight_termination_disabled) {
+			if (status.failure_detector_status != 0) {
-					if (failure_status != 0) {
 
-						armed.force_failsafe = true;
+				armed.force_failsafe = true;
-						status_changed = true;
+				status_changed = true;
 
-						_in_flight_termination = true;
+				_in_flight_termination = true;
 
-						mavlink_log_critical(&mavlink_log_pub, "Attitude failure detected: force failsafe");
+				mavlink_log_critical(&mavlink_log_pub, "Attitude failure detected: force failsafe");
-						set_tune_override(TONE_PARACHUTE_RELEASE_TUNE);
+				set_tune_override(TONE_PARACHUTE_RELEASE_TUNE);
-					}
-				}
 			}
 		}
 

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;
 }