Move VTOL Quadchute logic to Commander (failure detector)

Signed-off-by: Silvan Fuhrer <[email protected]>

msg/vehicle_status.msg

@@ -78,6 +78,7 @@ bool is_vtol_tailsitter                 # True if the system performs a 90° pit
 bool vtol_fw_permanent_stab		# True if VTOL should stabilize attitude for fw in manual mode
 bool in_transition_mode			# True if VTOL is doing a transition
 bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW
+bool vtol_fw_actuation_failure          # True if an issue with fixed-wing actuation was detected
 
 bool rc_signal_lost				# true if RC reception lost
 

msg/vehicle_status_flags.msg

@@ -38,7 +38,6 @@ bool offboard_control_signal_lost
 bool rc_signal_found_once
 bool rc_calibration_in_progress
 bool rc_calibration_valid                            # set if RC calibration is valid
-bool vtol_transition_failure                        # Set to true if vtol transition failed
 bool usb_connected                                # status of the USB power supply
 bool sd_card_detected_once                        # set to true if the SD card was detected
 

src/modules/commander/Commander.cpp

@@ -2259,11 +2259,6 @@ Commander::run()
 					_status_changed = true;
 				}
 
-				if (_status_flags.vtol_transition_failure != _vtol_status.vtol_transition_failsafe) {
-					_status_flags.vtol_transition_failure = _vtol_status.vtol_transition_failsafe;
-					_status_changed = true;
-				}
-
 				const bool should_soft_stop = (_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
 
 				if (_armed.soft_stop != should_soft_stop) {
@@ -2768,7 +2763,32 @@ Commander::run()
 					}
 				}
 
-				if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) {
+				const bool flight_state_enable_quadchute = _status.is_vtol
+						&& (_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _status.in_transition_mode);
+
+				if (flight_state_enable_quadchute && !_quadchute_triggered) {
+					if (fd_status_flags.qc_roll || fd_status_flags.qc_pitch || fd_status_flags.qc_min_alt || fd_status_flags.qc_alt_err
+					    || fd_status_flags.qc_trans_tmt) {
+						_quadchute_triggered = true;
+						mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: Transition to hover\t");
+						/* EVENT
+						 * @description
+						 * Critical failures include an exceeding tilt angle, altitude failure or an external failure trigger.
+						 *
+						 * <profile name="dev">
+						 * Quadchute can be disabled with the parameter <param>QC_enabled</param>.
+						 * </profile>
+						 */
+						events::send(events::ID("commander_fd_quadchute"), {events::Log::Emergency, events::LogInternal::Warning},
+							     "Critical failure detected: transition to hover");
+						// send_quadchute_command();
+						send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION, vehicle_status_s::VEHICLE_TYPE_ROTARY_WING,
+								     1.0f);
+						_status.vtol_fw_actuation_failure = true;
+						_status_changed = true;
+					}
+
+				} else if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) {
 					const bool is_right_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get());
 
 					if (is_right_after_takeoff && !_lockdown_triggered) {
@@ -4389,6 +4409,18 @@ void Commander::send_parachute_command()
 	set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
 }
 
+// void Commander::send_quadchute_command()
+// {
+// 	uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)};
+// 		vehicle_status_s vehicle_status{};
+// 		vehicle_status_sub.copy(&vehicle_status);
+// 		send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
+// 				     (float)(vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ?
+// 					     vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW :
+// 					     vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC), 0.0f);
+
+// }
+
 void Commander::checkWindAndWarn()
 {
 	wind_s wind_estimate;

src/modules/commander/Commander.hpp

@@ -322,7 +322,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	bool		_flight_termination_triggered{false};
 	bool		_lockdown_triggered{false};
 	bool            _imbalanced_propeller_check_triggered{false};
-
+	bool		_quadchute_triggered{false};
 
 	hrt_abstime	_datalink_last_heartbeat_gcs{0};
 	hrt_abstime	_datalink_last_heartbeat_avoidance_system{0};

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -52,7 +52,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 	failure_detector_status_u status_prev = _status;
 
 	if (vehicle_control_mode.flag_control_attitude_enabled) {
-		updateAttitudeStatus();
+		updateAttitudeStatus(vehicle_status);
 
 		if (_param_fd_ext_ats_en.get()) {
 			updateExternalAtsStatus();
@@ -73,10 +73,81 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 		updateImbalancedPropStatus();
 	}
 
+
+	if (vehicle_status.is_vtol) {
+		updateMinHeightStatus(vehicle_status);
+		updateAdaptiveQC(vehicle_status, vehicle_control_mode);
+		updateTransitionTimeout();
+	}
+
 	return _status.value != status_prev.value;
 }
 
-void FailureDetector::updateAttitudeStatus()
+void FailureDetector::updateMinHeightStatus(const vehicle_status_s &vehicle_status)
+{
+	vehicle_local_position_s vehicle_local_position;
+
+	if (_vehicle_local_position_sub.update(&vehicle_local_position)) {
+		if (vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+		    && _param_vt_fw_min_alt.get() > 0
+		    && -(vehicle_local_position.z) < _param_vt_fw_min_alt.get()) {
+			_status.flags.qc_min_alt = true;
+
+		} else {
+			_status.flags.qc_min_alt = false;
+		}
+	}
+}
+
+void FailureDetector::updateAdaptiveQC(const vehicle_status_s &vehicle_status,
+				       const vehicle_control_mode_s &vehicle_control_mode)
+{
+	// adaptive quadchute, only enabled when TECS is running (so pure FW mode)
+	if (_param_vt_fw_alt_err.get() > 0 && vehicle_control_mode.flag_control_climb_rate_enabled
+	    && vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		tecs_status_s tecs_status;
+
+		if (_tecs_status_sub.update(&tecs_status)) {
+			// 1 second rolling average
+			_ra_hrate = (49 * _ra_hrate + tecs_status.height_rate) / 50;
+			_ra_hrate_sp = (49 * _ra_hrate_sp + tecs_status.height_rate_setpoint) / 50;
+
+			// are we dropping while requesting significant ascend?
+			if (((tecs_status.altitude_sp - tecs_status.altitude_filtered) > _param_vt_fw_alt_err.get()) &&
+			    (_ra_hrate < -1.0f) && (_ra_hrate_sp > 1.0f)) {
+
+				_status.flags.qc_alt_err = true;
+			}
+		}
+
+	} else {
+		// reset the filtered height rate and heigh rate setpoint if TECS is not running
+		_ra_hrate = 0.0f;
+		_ra_hrate_sp = 0.0f;
+	}
+}
+
+void FailureDetector::updateTransitionTimeout()
+{
+	vtol_vehicle_status_s vtol_vehicle_status;
+	_vtol_vehicle_status_sub.update(&vtol_vehicle_status);
+
+	const bool transitioning_to_FW = vtol_vehicle_status.in_transition_to_fw;
+
+	if (!_was_in_transition_FW_prev && transitioning_to_FW) {
+		_transition_start_timestamp = hrt_absolute_time();
+		_was_in_transition_FW_prev = true;
+	}
+
+	if (transitioning_to_FW && _param_vt_trans_timeout.get() > FLT_EPSILON) {
+		if (hrt_elapsed_time(&_transition_start_timestamp) > _param_vt_trans_timeout.get() * 1_s) {
+			// transition timeout occured, abort transition
+			_status.flags.qc_trans_tmt = true;
+		}
+	}
+}
+
+void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_status)
 {
 	vehicle_attitude_s attitude;
 
@@ -105,6 +176,31 @@ void FailureDetector::updateAttitudeStatus()
 		// Update status
 		_status.flags.roll = _roll_failure_hysteresis.get_state();
 		_status.flags.pitch = _pitch_failure_hysteresis.get_state();
+
+		const bool enable_quadchte = vehicle_status.is_vtol
+					     && vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+
+		if (enable_quadchte) {
+
+			// fixed-wing maximum roll angle
+			if (_param_vt_fw_qc_r.get() > 0) {
+				if (fabsf(roll) > fabsf(math::radians(_param_vt_fw_qc_r.get()))) {
+					_status.flags.qc_roll = true;
+				}
+			}
+
+			// fixed-wing maximum pitch angle
+			if (_param_vt_fw_qc_p.get() > 0) {
+				if (fabsf(pitch) > fabsf(math::radians(_param_vt_fw_qc_p.get()))) {
+					_status.flags.qc_pitch = true;
+				}
+			}
+
+		} else {
+			// reset if not in transition or FW
+			_status.flags.qc_roll = true;
+			_status.flags.qc_pitch = true;
+		}
 	}
 }
 

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -1,7 +1,7 @@
 
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -52,11 +52,15 @@
 // subscriptions
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_selection.h>
+#include <uORB/topics/tecs_status.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_imu_status.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/pwm_input.h>
 
@@ -70,6 +74,11 @@ union failure_detector_status_u {
 		uint16_t high_wind : 1;
 		uint16_t battery : 1;
 		uint16_t imbalanced_prop : 1;
+		uint16_t qc_roll : 1;
+		uint16_t qc_pitch : 1;
+		uint16_t qc_min_alt : 1;
+		uint16_t qc_alt_err : 1;
+		uint16_t qc_trans_tmt : 1;
 	} flags;
 	uint16_t value {0};
 };
@@ -87,10 +96,13 @@ class FailureDetector : public ModuleParams
 	float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
 
 private:
-	void updateAttitudeStatus();
+	void updateAttitudeStatus(const vehicle_status_s &vehicle_status);
 	void updateExternalAtsStatus();
 	void updateEscsStatus(const vehicle_status_s &vehicle_status);
 	void updateImbalancedPropStatus();
+	void updateMinHeightStatus(const vehicle_status_s &vehicle_status);
+	void updateAdaptiveQC(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
+	void updateTransitionTimeout();
 
 	failure_detector_status_u _status{};
 
@@ -110,6 +122,17 @@ class FailureDetector : public ModuleParams
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
 
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint});
+	uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
+	uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
+
+	float _ra_hrate{0.f};
+	float _ra_hrate_sp{0.f};
+
+	hrt_abstime _transition_start_timestamp{0};
+	bool _was_in_transition_FW_prev{false};
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
 		(ParamInt<px4::params::FD_FAIL_R>) _param_fd_fail_r,
@@ -118,6 +141,12 @@ class FailureDetector : public ModuleParams
 		(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
 		(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
 		(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
-		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
+		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr,
+
+		(ParamInt<px4::params::VT_FW_MIN_ALT>) _param_vt_fw_min_alt,
+		(ParamInt<px4::params::VT_FW_ALT_ERR>) _param_vt_fw_alt_err,
+		(ParamInt<px4::params::VT_FW_QC_P>) _param_vt_fw_qc_p,
+		(ParamInt<px4::params::VT_FW_QC_R>) _param_vt_fw_qc_r,
+		(ParamFloat<px4::params::VT_TRANS_TIMEOUT>) _param_vt_trans_timeout
 	)
 };

src/modules/commander/failure_detector/failure_detector_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -156,3 +156,63 @@ PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
  * @group Failure Detector
  */
 PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30);
+
+/**
+ * QuadChute minimum height threshold
+ *
+ * If the current height (relative to takeoff point) drops below this value in fixed-wing mode, a "QuadChute" is triggered,
+ * which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT.
+ * Only enabled for VTOL vehicles.
+ * Set to 0 to disable the check.
+ *
+ * @min 0
+ * @max 500
+ * @unit m
+ * @increment 1
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(VT_FW_MIN_ALT, 0);
+
+/**
+ * QuadChute altitude error thershold
+ *
+ * If the current altitude is more than this amount below the setpoint, a "QuadChute" is triggered,
+ * which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT.
+ * Only enabled for VTOL vehicles.
+ * Set to 0 to disable the check.
+ *
+ * @min 0
+ * @max 200
+ * @unit m
+ * @increment 1
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(VT_FW_ALT_ERR, 0);
+
+/**
+ * QuadChute pitch threshold
+ *
+ * If the current pitch angle estimate is above this value, a "QuadChute" is triggered,
+ * which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT.
+ * Set to 0 to disable the check.
+ *
+ * @min 0
+ * @max 180
+ * @unit deg
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(VT_FW_QC_P, 0);
+
+/**
+ * QuadChute roll threshold
+ *
+ * If the current roll angle estimate is above this value, a "QuadChute" is triggered,
+ * which immediately switches the vehicle to hover flight and executes the action defined in COM_QC_ACT.
+ * Set to 0 to disable the check.
+ *
+ * @min 0
+ * @max 180
+ * @unit deg
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(VT_FW_QC_R, 0);