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;