From eae939d86c77fd5419bc8811792eadb3b5274740 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 20 Nov 2023 16:30:22 +0100 Subject: [PATCH] vtol_type: transition altitude loss quad-chute: handle z resets Signed-off-by: Silvan Fuhrer --- src/modules/vtol_att_control/vtol_type.cpp | 6 ++++++ src/modules/vtol_att_control/vtol_type.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index bf1ca82a9a..ba73688473 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -302,6 +302,12 @@ bool VtolType::isFrontTransitionAltitudeLoss() { bool result = false; + // check if there is a reset in the z-direction, and if so, shift the transition start z as well + if (_local_pos->z_reset_counter != _altitude_reset_counter) { + _local_position_z_start_of_transition += _local_pos->delta_z; + _altitude_reset_counter = _local_pos->z_reset_counter; + } + // only run if param set, altitude valid and controlled, and in transition to FW or within 5s of finishing it. if (_param_vt_qc_t_alt_loss.get() > FLT_EPSILON && _local_pos->z_valid && _v_control_mode->flag_control_altitude_enabled && (_common_vtol_mode == mode::TRANSITION_TO_FW || hrt_elapsed_time(&_trans_finished_ts) < 5_s)) { diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 93a2ff4494..5a89e5fa91 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -330,6 +330,8 @@ protected: float _local_position_z_start_of_transition{0.f}; // altitude at start of transition + int _altitude_reset_counter{0}; + DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, (ParamBool) _param_vt_elev_mc_lock, (ParamFloat) _param_vt_fw_min_alt,