From 9ed51ba8ed826cd1517ef335e98d1d78cf20c9f6 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 8 Dec 2022 12:05:38 +0300 Subject: [PATCH] quadchute: introduced parameter to specify maximum height Signed-off-by: RomanBapst --- .../vtol_att_control/vtol_att_control_params.c | 15 +++++++++++++++ src/modules/vtol_att_control/vtol_type.cpp | 16 +++++++++++++++- src/modules/vtol_att_control/vtol_type.h | 1 + 3 files changed, 31 insertions(+), 1 deletion(-) 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 bd32ff7947..393265cfd9 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -248,6 +248,21 @@ PARAM_DEFINE_INT32(VT_FW_QC_P, 0); */ PARAM_DEFINE_INT32(VT_FW_QC_R, 0); +/** + * Quadchute maximum height. + * + * Maximum height above the ground (if available, otherwhise above home if available, otherwise above the local origin) where triggering a quadchute is possible. + * Triggering a quadchute always means transitioning the vehicle to hover flight in which generally a lot of energy is consumed. + * At high altitudes there is therefore a big risk to deplete the battery and therefore crash. Currently, there is no automated + * re-transition to fixed wing mode implemented and therefore this parameter serves and an intermediate measure to increase safety. + * + * @unit m + * @increment 1 + * @decimal 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_QC_HMAX, 200.0f); + /** * 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 96f1fa592d..a48050eb36 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -214,7 +214,21 @@ bool VtolType::can_transition_on_ground() bool VtolType::isQuadchuteEnabled() { - return _v_control_mode->flag_armed && !_land_detected->landed; + float dist_to_ground = 0.f; + const float home_position_z = _attc->get_home_position_z(); + + if (_local_pos->dist_bottom_valid) { + dist_to_ground = _local_pos->dist_bottom; + + } else if (PX4_ISFINITE(home_position_z)) { + dist_to_ground = -(_local_pos->z - home_position_z); + + } else { + dist_to_ground = -_local_pos->z; + + } + + return _v_control_mode->flag_armed && !_land_detected->landed && dist_to_ground < _param_quadchute_max_height.get(); } bool VtolType::isMinAltBreached() diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index c3363665f7..8b520f753b 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -314,6 +314,7 @@ protected: (ParamFloat) _param_vt_fw_alt_err, (ParamInt) _param_vt_fw_qc_p, (ParamInt) _param_vt_fw_qc_r, + (ParamFloat) _param_quadchute_max_height, (ParamFloat) _param_vt_f_tr_ol_tm, (ParamFloat) _param_vt_trans_min_tm,