Failure detector / Failsafe (Parachute) triggering

ROMFS/px4fmu_common/mixers/quad_x.main.mix

@@ -1,5 +1,16 @@
 R: 4x 10000 10000 10000 0
+
+AUX1 Passthrough
 M: 1
 S: 3 5  10000  10000      0 -10000  10000
+
+AUX2 Passthrough
 M: 1
 S: 3 6  10000  10000      0 -10000  10000
+
+Failsafe outputs
+The following outputs are set to their disarmed value
+during normal operation and to their failsafe falue in case
+of flight termination.
+Z:
+Z:

src/drivers/drv_mixer.h

@@ -71,11 +71,6 @@
 /** reset (clear) the mixer configuration */
 #define MIXERIOCRESET		_MIXERIOC(1)
 
-/**
- * add a simple mixer in (struct mixer_simple_s *)arg
- */
-#define MIXERIOCADDSIMPLE	_MIXERIOC(2)
-
 /* _MIXERIOC(3) was deprecated */
 /* _MIXERIOC(4) was deprecated */
 

src/drivers/mkblctrl/mkblctrl.cpp

@@ -1052,27 +1052,6 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 		break;
 
-	case MIXERIOCADDSIMPLE: {
-			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
-
-			SimpleMixer *mixer = new SimpleMixer(control_callback,
-							     (uintptr_t)&_controls, mixinfo);
-
-			if (mixer->check()) {
-				delete mixer;
-				ret = -EINVAL;
-
-			} else {
-				if (_mixers == nullptr)
-					_mixers = new MixerGroup(control_callback,
-								 (uintptr_t)&_controls);
-
-				_mixers->add_mixer(mixer);
-			}
-
-			break;
-		}
-
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strnlen(buf, 1024);

src/drivers/pwm_out_sim/PWMSim.cpp

@@ -517,28 +517,6 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 		break;
 
-	case MIXERIOCADDSIMPLE: {
-			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
-
-			SimpleMixer *mixer = new SimpleMixer(control_callback, (uintptr_t)&_controls, mixinfo);
-
-			if (mixer->check()) {
-				delete mixer;
-				_groups_required = 0;
-				ret = -EINVAL;
-
-			} else {
-				if (_mixers == nullptr) {
-					_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
-				}
-
-				_mixers->add_mixer(mixer);
-				_mixers->groups_required(_groups_required);
-			}
-
-			break;
-		}
-
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strnlen(buf, 1024);

src/drivers/px4fmu/fmu.cpp

@@ -2152,28 +2152,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 		break;
 
-	case MIXERIOCADDSIMPLE: {
-			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
-
-			SimpleMixer *mixer = new SimpleMixer(control_callback, (uintptr_t)_controls, mixinfo);
-
-			if (mixer->check()) {
-				delete mixer;
-				_groups_required = 0;
-				ret = -EINVAL;
-
-			} else {
-				if (_mixers == nullptr) {
-					_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
-				}
-
-				_mixers->add_mixer(mixer);
-				_mixers->groups_required(_groups_required);
-			}
-
-			break;
-		}
-
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strnlen(buf, 1024);

src/drivers/px4io/px4io.cpp

@@ -181,14 +181,6 @@ class PX4IO : public cdev::CDev
 	*/
 	int      		set_update_rate(int rate);
 
-	/**
-	 * Push failsafe values to IO.
-	 *
-	 * @param[in] vals	Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
-	 * @param[in] len	Number of channels, could up to 8
-	 */
-	int			set_failsafe_values(const uint16_t *vals, unsigned len);
-
 	/**
 	 * Disable RC input handling
 	 */
@@ -734,17 +726,56 @@ PX4IO::init()
 			errx(1, "PRM CMPID");
 		}
 
-		/* send command to arm system via command API */
+		/* prepare vehicle command */
 		vehicle_command_s vcmd = {};
-		vcmd.timestamp = hrt_absolute_time();
-		vcmd.param1 = 1.0f; /* request arming */
-		vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
 		vcmd.target_system = (uint8_t)sys_id;
 		vcmd.target_component = (uint8_t)comp_id;
 		vcmd.source_system = (uint8_t)sys_id;
 		vcmd.source_component = (uint8_t)comp_id;
 		vcmd.confirmation = true; /* ask to confirm command */
 
+		if (reg & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
+			mavlink_log_emergency(&_mavlink_log_pub, "IO is in failsafe, force failsafe");
+			/* send command to terminate flight via command API */
+			vcmd.timestamp = hrt_absolute_time();
+			vcmd.param1 = 1.0f; /* request flight termination */
+			vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;
+
+			/* send command once */
+			orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+
+			/* spin here until IO's state has propagated into the system */
+			do {
+				orb_check(safety_sub, &updated);
+
+				if (updated) {
+					orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
+				}
+
+				/* wait 50 ms */
+				px4_usleep(50000);
+
+				/* abort after 5s */
+				if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) {
+					mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (3), abort");
+					return 1;
+				}
+
+				/* re-send if necessary */
+				if (!safety.force_failsafe) {
+					orb_publish(ORB_ID(vehicle_command), pub, &vcmd);
+					PX4_WARN("re-sending flight termination cmd");
+				}
+
+				/* keep waiting for state change for 2 s */
+			} while (!safety.force_failsafe);
+		}
+
+		/* send command to arm system via command API */
+		vcmd.timestamp = hrt_absolute_time();
+		vcmd.param1 = 1.0f; /* request arming */
+		vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
+
 		/* send command once */
 		orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
 
@@ -1031,21 +1062,15 @@ PX4IO::task_main()
 					}
 				}
 
-				int32_t safety_param_val;
-				param_t safety_param = param_find("CBRK_IO_SAFETY");
-
-				if (safety_param != PARAM_INVALID) {
-
-					param_get(safety_param, &safety_param_val);
-
-					if (safety_param_val == PX4IO_FORCE_SAFETY_MAGIC) {
-						/* disable IO safety if circuit breaker asked for it */
-						(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, safety_param_val);
-					}
-				}
+				/* Check if the IO safety circuit breaker has been updated */
+				bool circuit_breaker_io_safety_enabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
+				/* Bypass IO safety switch logic by setting FORCE_SAFETY_OFF */
+				(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, circuit_breaker_io_safety_enabled);
 
 				/* Check if the flight termination circuit breaker has been updated */
 				_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
+				/* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */
+				(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION, !_cb_flighttermination);
 
 				param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan);
 				param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max);
@@ -1385,8 +1410,7 @@ PX4IO::io_set_arming_state()
 			_lockdown_override = false;
 		}
 
-		/* Do not set failsafe if circuit breaker is enabled */
-		if (armed.force_failsafe && !_cb_flighttermination) {
+		if (armed.force_failsafe) {
 			set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
 
 		} else {
@@ -2217,7 +2241,7 @@ PX4IO::print_status(bool extended_status)
 	       ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""),
 	       ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT)  ? " VSERVO_FAULT" : ""));
 	/* now clear alarms */
-	io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
+	io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0x0000);
 
 	if (_hardware == 2) {
 		printf("vservo %u mV vservo scale %u\n",

src/lib/circuit_breaker/circuit_breaker_params.c

@@ -103,9 +103,9 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
 /**
  * Circuit breaker for flight termination
  *
- * Setting this parameter to 121212 will disable the flight termination action.
- * --> The IO driver will not do flight termination if requested by the FMU
- * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ * Setting this parameter to 121212 will disable the flight termination action if triggered
+ * by the FailureDetector logic or if FMU is lost.
+ * This circuit breaker does not affect the RC loss, data link loss and geofence safety logic.
  *
  * @reboot_required true
  * @min 0

src/modules/commander/Commander.cpp

@@ -2206,27 +2206,32 @@ Commander::run()
 		}
 
 		/* Check for failure detector status */
-		if (armed.armed) {
+		const bool failure_detector_updated = _failure_detector.update();
 
-			if (_failure_detector.update()) {
+		if (failure_detector_updated) {
 
-				const uint8_t failure_status = _failure_detector.get_status();
+			const uint8_t failure_status = _failure_detector.getStatus();
 
-				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 (failure_status != 0 && !status_flags.circuit_breaker_flight_termination_disabled) {
+		if (armed.armed &&
+		    failure_detector_updated &&
+		    !_flight_termination_triggered &&
+		    !status_flags.circuit_breaker_flight_termination_disabled) {
 
-					// TODO: set force_failsafe flag
+			if (_failure_detector.isFailure()) {
 
-					if (!_failure_detector_termination_printed) {
-						mavlink_log_critical(&mavlink_log_pub, "Attitude failure detected! Enforcing failsafe");
-						_failure_detector_termination_printed = true;
-					}
+				armed.force_failsafe = true;
+				status_changed = true;
 
-				}
+				_flight_termination_triggered = true;
+
+				mavlink_log_critical(&mavlink_log_pub, "Critical failure detected: terminate flight");
+				set_tune_override(TONE_PARACHUTE_RELEASE_TUNE);
 			}
 		}
 

src/modules/commander/Commander.hpp

@@ -169,7 +169,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	bool _geofence_violated_prev{false};
 
 	FailureDetector _failure_detector;
-	bool _failure_detector_termination_printed{false};
+	bool _flight_termination_triggered{false};
 
 	bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
 			    orb_advert_t *command_ack_pub, bool *changed);

src/modules/commander/PreflightCheck.cpp

@@ -674,6 +674,38 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s
 	return success;
 }
 
+static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, bool report_fail,
+				 bool prearm)
+{
+	bool success = true;
+
+	if (!prearm) {
+		// Ignore failure detector check after arming.
+		return true;
+
+	}
+
+	if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) {
+		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 +721,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 +977,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;
 }