From f3cdf70732c2b1ec24c8017aec1941e2295cdc7b Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 12 Jan 2023 17:07:26 +0100 Subject: [PATCH] VTOL: Quad-chute: rework loss of altitude condition Previously the condition was based on hard coded height rate estimate and setpoint values and an altitude error threshold. That showed to be leading to false positives when the vehicle doesn't tightly follow the altitdue ramp given by TECS to achieve a new altitude setpoint, and has become completely infeasibly with the latest TECS rework that leads to non-ramped altitude setpoint changes in the tecs_status message. The new check no longer checks the altitude error but only the height rate instead. It begins to integrate the height rate error once it detects an uncommanded descend condition (height rate negative while setpoint is positive). Integral threshold can be tuned by user (VT_QC_HR_ERROR_I). Signed-off-by: Silvan Fuhrer --- .../vtol_att_control_main.cpp | 6 +-- .../vtol_att_control_params.c | 12 +++-- src/modules/vtol_att_control/vtol_type.cpp | 47 +++++++++---------- src/modules/vtol_att_control/vtol_type.h | 21 ++++----- 4 files changed, 40 insertions(+), 46 deletions(-) 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 3830932ff1..30a2a0a262 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -213,10 +213,10 @@ VtolAttitudeControl::quadchute(QuadchuteReason reason) "Quad-chute triggered due to minimum altitude breach"); break; - case QuadchuteReason::LossOfAlt: - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: loss of altitude\t"); + case QuadchuteReason::UncommandedDescent: + mavlink_log_critical(&_mavlink_log_pub, "Quadchute: Uncommanded descent detected\t"); events::send(events::ID("vtol_att_ctrl_quadchute_alt_loss"), events::Log::Critical, - "Quad-chute triggered due to loss of altitude"); + "Quad-chute triggered due to uncommanded descent detection"); break; case QuadchuteReason::TransitionAltitudeLoss: 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 85be3cbf97..03b61f497e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -212,10 +212,14 @@ PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f); PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f); /** - * Adaptive QuadChute + * Quad-chute uncommanded descent threshold * - * Maximum negative altitude error for fixed wing flight. If the altitude drops below this value below the altitude setpoint - * the vehicle will transition back to MC mode and enter failsafe RTL. + * 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. + * + * Set to 0 do disable this threshold. * * @unit m * @min 0.0 @@ -224,7 +228,7 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f); * @decimal 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f); +PARAM_DEFINE_FLOAT(VT_QC_HR_ERROR_I, 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 0226a880f8..a7055c6d89 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -257,17 +257,28 @@ bool VtolType::isMinAltBreached() return false; } -bool VtolType::largeAltitudeLoss() +bool VtolType::isUncommandedDescent() { - // adaptive quadchute - if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled && _tecs_running) { + if (_param_vt_qc_hr_error_i.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled + && hrt_elapsed_time(&_tecs_status->timestamp) < 1_s) { - // 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)) { - return true; + // TODO if TECS publishes local_position_setpoint dependency on tecs_status can be dropped here + + if (_tecs_status->height_rate < -FLT_EPSILON && _tecs_status->height_rate_setpoint > FLT_EPSILON) { + // vehicle is currently in uncommended descend, start integrating error + + 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()); } return false; @@ -340,8 +351,8 @@ QuadchuteReason VtolType::getQuadchuteReason() return QuadchuteReason::MinimumAltBreached; } - if (largeAltitudeLoss()) { - return QuadchuteReason::LossOfAlt; + if (isUncommandedDescent()) { + return QuadchuteReason::UncommandedDescent; } if (isFrontTransitionAltitudeLoss()) { @@ -363,20 +374,6 @@ QuadchuteReason VtolType::getQuadchuteReason() return QuadchuteReason::None; } -void VtolType::filterTecsHeightRates() -{ - if (_tecs_running) { - // 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; - - } 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 VtolType::handleSpecialExternalCommandQuadchute() { if (_attc->get_transition_command() == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC && _attc->get_immediate_transition() @@ -392,8 +389,6 @@ void VtolType::handleSpecialExternalCommandQuadchute() void VtolType::check_quadchute_condition() { - - filterTecsHeightRates(); handleSpecialExternalCommandQuadchute(); if (isQuadchuteEnabled()) { diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index e062ec340d..3f9ba2ade9 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -89,7 +89,7 @@ enum class QuadchuteReason { TransitionTimeout, ExternalCommand, MinimumAltBreached, - LossOfAlt, + UncommandedDescent, TransitionAltitudeLoss, MaximumPitchExceeded, MaximumRollExceeded, @@ -165,12 +165,11 @@ public: bool isMinAltBreached(); /** - * @brief Indicates if the vehicle has an altitude error larger than VT_FW_ALT_ERR and is losing altitude quickly. - * This only applies when TECS is running. + * @brief Indicates if conditions are met for uncommanded-descent quad-chute. * - * @return true if error larger than threshold + * @return true if integrated height rate error larger than threshold */ - bool largeAltitudeLoss(); + bool isUncommandedDescent(); /** * @brief Indicates if there is an altitude loss higher than specified threshold during a VTOL transition to FW @@ -200,11 +199,6 @@ public: */ bool isFrontTransitionTimeout(); - /** - * @brief Applied a first order low pass filte to TECS height rate and heigh rate setpoint. - */ - void filterTecsHeightRates(); - /** * @brief Special handling of QuadchuteReason::ReasonExternal */ @@ -290,8 +284,8 @@ 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 _ra_hrate = 0.0f; // rolling average on height rate for quadchute condition - float _ra_hrate_sp = 0.0f; // rolling average on height rate setpoint for quadchute condition + float _height_rate_error_integral{0.f}; + hrt_abstime _trans_finished_ts = 0; hrt_abstime _transition_start_timestamp{0}; @@ -302,6 +296,7 @@ protected: hrt_abstime _last_loop_ts = 0; float _transition_dt = 0; + hrt_abstime _last_loop_quadchute_timestamp = 0; float _accel_to_pitch_integ = 0; @@ -319,7 +314,7 @@ protected: DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, (ParamBool) _param_vt_elev_mc_lock, (ParamFloat) _param_vt_fw_min_alt, - (ParamFloat) _param_vt_fw_alt_err, + (ParamFloat) _param_vt_qc_hr_error_i, (ParamInt) _param_vt_fw_qc_p, (ParamInt) _param_vt_fw_qc_r, (ParamFloat) _param_vt_qc_t_alt_loss,