From b60e73c76f8da7e2d18569f71163b08ca8e98fac Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 18 Jan 2024 10:55:28 +0300 Subject: [PATCH] bad descend quadchute: take altitude reset into account (#22643) - apply delta from reset to reference altitude state to avoid false triggering Signed-off-by: RomanBapst --- src/modules/vtol_att_control/vtol_type.cpp | 11 ++++++++++- src/modules/vtol_att_control/vtol_type.h | 2 +- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 6fd14359f5..45f655101d 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -286,13 +286,17 @@ bool VtolType::isUncommandedDescent() && (current_altitude < _tecs_status->altitude_reference) && hrt_elapsed_time(&_tecs_status->timestamp) < 1_s) { + if (!PX4_ISFINITE(_quadchute_ref_alt)) { + _quadchute_ref_alt = current_altitude; + } + _quadchute_ref_alt = math::min(math::max(_quadchute_ref_alt, current_altitude), _tecs_status->altitude_reference); return (_quadchute_ref_alt - current_altitude) > _param_vt_qc_alt_loss.get(); } else { - _quadchute_ref_alt = -MAXFLOAT; + _quadchute_ref_alt = NAN; } return false; @@ -318,6 +322,11 @@ void VtolType::handleEkfResets() 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; + + if (PX4_ISFINITE(_quadchute_ref_alt)) { + _quadchute_ref_alt += _local_pos->delta_z; + } + } } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 3707acb68e..d59887372e 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -316,7 +316,7 @@ protected: hrt_abstime _last_loop_ts = 0; float _transition_dt = 0; - float _quadchute_ref_alt{-MAXFLOAT}; // altitude (AMSL) reference to compute quad-chute altitude loss condition + float _quadchute_ref_alt{NAN}; // altitude (AMSL) reference to compute quad-chute altitude loss condition float _accel_to_pitch_integ = 0;