From 5aa5fc2da5d97b25427235cf1838e269e7217352 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 31 May 2023 18:57:50 +0300 Subject: [PATCH] Refactor uncommanded descend Quad-Chute (#21598) * refactored uncommanded descend quadchute - use fixed altitude error threshold - compute error relative to higest altitude the vehicle has achieved since it has flown below the altitude reference (setpoint) * disabled altitude loss quadchute by default * altitude loss quadchute: added protection against invalid local z --------- Signed-off-by: RomanBapst --- .../vtol_att_control_params.c | 12 ++++----- src/modules/vtol_att_control/vtol_type.cpp | 27 ++++++++----------- src/modules/vtol_att_control/vtol_type.h | 8 +++--- 3 files changed, 20 insertions(+), 27 deletions(-) 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 3c8d3c4719..1361f1f5e6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -197,12 +197,12 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f); /** * Quad-chute uncommanded descent threshold * - * Threshold for integrated height rate error to trigger a uncommanded-descent quad-chute. - * Only checked in altitude-controlled fixed-wing flight. - * Additional conditions that have to be met for uncommanded descent detection are a positive (climbing) height - * rate setpoint and a negative (sinking) current height rate estimate. + * Altitude error threshold for quad-chute triggering during fixed-wing flight. + * The check is only active if altitude is controlled and the vehicle is below the current altitude reference. + * The altitude error is relative to the highest altitude the vehicle has achieved since it has flown below the current + * altitude reference. * - * Set to 0 do disable this threshold. + * Set to 0 do disable. * * @unit m * @min 0.0 @@ -211,7 +211,7 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f); * @decimal 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_QC_HR_ERROR_I, 0.0f); +PARAM_DEFINE_FLOAT(VT_QC_ALT_LOSS, 0.0f); /** * Quad-chute transition altitude loss threshold diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 2f7c955285..92c3672f0a 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -272,26 +272,21 @@ bool VtolType::isMinAltBreached() bool VtolType::isUncommandedDescent() { - if (_param_vt_qc_hr_error_i.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled + const float current_altitude = -_local_pos->z + _local_pos->ref_alt; + + if (_param_vt_qc_alt_loss.get() > FLT_EPSILON && _local_pos->z_valid && _local_pos->z_global + && _v_control_mode->flag_control_altitude_enabled + && PX4_ISFINITE(_tecs_status->altitude_reference) + && (current_altitude < _tecs_status->altitude_reference) && hrt_elapsed_time(&_tecs_status->timestamp) < 1_s) { - // TODO if TECS publishes local_position_setpoint dependency on tecs_status can be dropped here + _quadchute_ref_alt = math::min(math::max(_quadchute_ref_alt, current_altitude), + _tecs_status->altitude_reference); - if (_tecs_status->height_rate < -FLT_EPSILON && _tecs_status->height_rate_setpoint > FLT_EPSILON) { - // vehicle is currently in uncommended descend, start integrating error + return (_quadchute_ref_alt - current_altitude) > _param_vt_qc_alt_loss.get(); - const hrt_abstime now = hrt_absolute_time(); - float dt = static_cast(now - _last_loop_quadchute_timestamp) / 1e6f; - dt = math::constrain(dt, 0.0001f, 0.1f); - _last_loop_quadchute_timestamp = now; - - _height_rate_error_integral += (_tecs_status->height_rate_setpoint - _tecs_status->height_rate) * dt; - - } else { - _height_rate_error_integral = 0.f; // reset - } - - return (_height_rate_error_integral > _param_vt_qc_hr_error_i.get()); + } else { + _quadchute_ref_alt = -MAXFLOAT; } return false; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 12703f5a24..d8f3de8620 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -288,9 +288,6 @@ protected: float _thrust_transition = 0.0f; // thrust value applied during a front transition (tailsitter & tiltrotor only) float _last_thr_in_fw_mode = 0.0f; - float _height_rate_error_integral{0.f}; - - hrt_abstime _trans_finished_ts = 0; hrt_abstime _transition_start_timestamp{0}; float _time_since_trans_start{0}; @@ -300,7 +297,8 @@ protected: hrt_abstime _last_loop_ts = 0; float _transition_dt = 0; - hrt_abstime _last_loop_quadchute_timestamp = 0; + + float _quadchute_ref_alt{-MAXFLOAT}; // altitude (AMSL) reference to compute quad-chute altitude loss condition float _accel_to_pitch_integ = 0; @@ -317,7 +315,7 @@ protected: DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, (ParamBool) _param_vt_elev_mc_lock, (ParamFloat) _param_vt_fw_min_alt, - (ParamFloat) _param_vt_qc_hr_error_i, + (ParamFloat) _param_vt_qc_alt_loss, (ParamInt) _param_vt_fw_qc_p, (ParamInt) _param_vt_fw_qc_r, (ParamFloat) _param_vt_qc_t_alt_loss,