VTOL QuadChute Maximum roll and pitch angles (#6665)

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -132,6 +132,8 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params_handles.vtol_type = param_find("VT_TYPE");
 	_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
 	_params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT");
+	_params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P");
+	_params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R");
 	_params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
 	_params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
 
@@ -562,6 +564,14 @@ VtolAttitudeControl::parameters_update()
 	param_get(_params_handles.fw_min_alt, &v);
 	_params.fw_min_alt = v;
 
+	/* maximum pitch angle (QuadChute) */
+	param_get(_params_handles.fw_qc_max_pitch, &l);
+	_params.fw_qc_max_pitch = l;
+
+	/* maximum roll angle (QuadChute) */
+	param_get(_params_handles.fw_qc_max_roll, &l);
+	_params.fw_qc_max_roll = l;
+
 	param_get(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop);
 
 	param_get(_params_handles.front_trans_time_min, &_params.front_trans_time_min);

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -210,6 +210,8 @@ private:
 		param_t vtol_type;
 		param_t elevons_mc_lock;
 		param_t fw_min_alt;
+		param_t fw_qc_max_pitch;
+		param_t fw_qc_max_roll;
 		param_t front_trans_time_openloop;
 		param_t front_trans_time_min;
 	} _params_handles;

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -297,7 +297,7 @@ PARAM_DEFINE_FLOAT(VT_TRANS_TIMEOUT, 15.0f);
 PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
 
 /**
- * QuadChute
+ * QuadChute Altitude
  *
  * Minimum altitude for fixed wing flight, when in fixed wing the altitude drops below this altitude
  * the vehicle will transition back to MC mode and enter failsafe RTL
@@ -307,6 +307,29 @@ PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
  */
 PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
 
+
+/**
+ * QuadChute Max Pith
+ *
+ * Maximum pitch angle before QuadChute engages
+ * Above this the vehicle will transition back to MC mode and enter failsafe RTL
+ * @min 0
+ * @max 180
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_FW_QC_P, 0);
+
+/**
+ * QuadChute Max Roll
+ *
+ * Maximum roll angle before QuadChute engages
+ * Above this the vehicle will transition back to MC mode and enter failsafe RTL
+ * @min 0
+ * @max 180
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_FW_QC_R, 0);
+
 /**
  * Airspeed less front transition time (open loop)
  *

src/modules/vtol_att_control/vtol_type.cpp

@@ -194,12 +194,33 @@ bool VtolType::can_transition_on_ground()
 
 void VtolType::check_quadchute_condition()
 {
-	// fixed-wing minimum altitude, armed, !landed
-	if (_params->fw_min_alt > FLT_EPSILON
-	    && _armed->armed && !_land_detected->landed) {
 
-		if (-(_local_pos->z) < _params->fw_min_alt) {
-			_attc->abort_front_transition("Minimum altitude");
+	if (_armed->armed && !_land_detected->landed) {
+		matrix::Eulerf euler = matrix::Quatf(_v_att->q);
+
+		// fixed-wing minimum altitude
+		if (_params->fw_min_alt > FLT_EPSILON) {
+
+			if (-(_local_pos->z) < _params->fw_min_alt) {
+				_attc->abort_front_transition("Minimum altitude breached");
+			}
+		}
+
+		// fixed-wing maximum pitch angle
+		if (_params->fw_qc_max_pitch > 0) {
+
+			if (fabs(euler.theta()) > fabs(math::radians(_params->fw_qc_max_pitch))) {
+				_attc->abort_front_transition("Maximum pitch angle exceeded");
+			}
+		}
+
+		// fixed-wing maximum roll angle
+		if (_params->fw_qc_max_roll > 0) {
+
+			if (fabs(euler.phi()) > fabs(math::radians(_params->fw_qc_max_roll))) {
+				_attc->abort_front_transition("Maximum roll angle exceeded");
+			}
 		}
 	}
+
 }

src/modules/vtol_att_control/vtol_type.h

@@ -60,6 +60,8 @@ struct Params {
 	int vtol_type;
 	int elevons_mc_lock;		// lock elevons in multicopter mode
 	float fw_min_alt;			// minimum relative altitude for FW mode (QuadChute)
+	float fw_qc_max_pitch;		// maximum pitch angle FW mode (QuadChute)
+	float fw_qc_max_roll;		// maximum roll angle FW mode (QuadChute)
 	float front_trans_time_openloop;
 	float front_trans_time_min;
 };