Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

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

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-11-20 16:30:22 +01:00
parent 8a899170bf
commit eae939d86c
2 changed files with 8 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/vtol_type.cpp
+6
View File
@@ -302,6 +302,12 @@ bool VtolType::isFrontTransitionAltitudeLoss()
{ {
bool result = false; 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. // 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 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)) { && (_common_vtol_mode == mode::TRANSITION_TO_FW || hrt_elapsed_time(&_trans_finished_ts) < 5_s)) {
src/modules/vtol_att_control/vtol_type.h
+2
View File
@@ -330,6 +330,8 @@ protected:
float _local_position_z_start_of_transition{0.f}; // altitude at start of transition float _local_position_z_start_of_transition{0.f}; // altitude at start of transition
int _altitude_reset_counter{0};
DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
(ParamBool<px4::params::VT_ELEV_MC_LOCK>) _param_vt_elev_mc_lock, (ParamBool<px4::params::VT_ELEV_MC_LOCK>) _param_vt_elev_mc_lock,
(ParamFloat<px4::params::VT_FW_MIN_ALT>) _param_vt_fw_min_alt, (ParamFloat<px4::params::VT_FW_MIN_ALT>) _param_vt_fw_min_alt,