From a99a7d2abdad4c8f804aabc3bbcddadff0ed8fda Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 21 Nov 2023 15:29:47 +0100 Subject: [PATCH] vtol: always check for EKF2 resets, not just when QC is checked Signed-off-by: Silvan Fuhrer --- .../vtol_att_control/vtol_att_control_main.cpp | 2 ++ src/modules/vtol_att_control/vtol_type.cpp | 15 +++++++++------ src/modules/vtol_att_control/vtol_type.h | 6 ++++++ 3 files changed, 17 insertions(+), 6 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 8dee7fd0c0..eccdf78305 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -373,6 +373,8 @@ VtolAttitudeControl::Run() _air_density = air_data.rho; } + _vtol_type->handleEkfResets(); + // check if mc and fw sp were updated const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index ba73688473..c44582a8dc 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -302,12 +302,6 @@ 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)) { @@ -318,6 +312,15 @@ bool VtolType::isFrontTransitionAltitudeLoss() return result; } +void VtolType::handleEkfResets() +{ + // 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; + } +} + bool VtolType::isPitchExceeded() { // fixed-wing maximum pitch angle diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 5a89e5fa91..8e347e1877 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -270,6 +270,12 @@ public: */ void resetTransitionStates(); + /** + * @brief Handle EKF position resets. + * + */ + void handleEkfResets(); + protected: VtolAttitudeControl *_attc; mode _common_vtol_mode;