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,