vtol_type: transition altitude loss quad-chute: handle z resets

Signed-off-by: Silvan Fuhrer <[email protected]>
PX4 · Nov 23, 2023 · eae939d · eae939d
1 parent 8a89917
commit eae939d
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 0 deletions.
diff --git 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
@@ -330,6 +330,8 @@ class VtolType : public ModuleParams
 
 	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<px4::params::VT_ELEV_MC_LOCK>) _param_vt_elev_mc_lock,
 					(ParamFloat<px4::params::VT_FW_MIN_ALT>) _param_vt_fw_min_alt,