diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 552911145c..e5258acf98 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/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); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index a43666c8c1..b23d8e7593 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/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; diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index dd83274cd7..fb8dcd13b9 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/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) * diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index fe1f50ba91..aabc53eccc 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/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"); + } } } + } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 1a6e863302..e3892b0014 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/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; };