dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <bapstroman@gmail.com>
This commit is contained in:
Roman Bapst 2024-01-18 10:55:28 +03:00 committed by GitHub
parent ed0d26de8a
commit b60e73c76f
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -286,13 +286,17 @@ bool VtolType::isUncommandedDescent()
&& (current_altitude < _tecs_status->altitude_reference) && (current_altitude < _tecs_status->altitude_reference)
&& hrt_elapsed_time(&_tecs_status->timestamp) < 1_s) { && 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), _quadchute_ref_alt = math::min(math::max(_quadchute_ref_alt, current_altitude),
_tecs_status->altitude_reference); _tecs_status->altitude_reference);
return (_quadchute_ref_alt - current_altitude) > _param_vt_qc_alt_loss.get(); return (_quadchute_ref_alt - current_altitude) > _param_vt_qc_alt_loss.get();
} else { } else {
_quadchute_ref_alt = -MAXFLOAT; _quadchute_ref_alt = NAN;
} }
return false; return false;
@ -318,6 +322,11 @@ void VtolType::handleEkfResets()
if (_local_pos->z_reset_counter != _altitude_reset_counter) { if (_local_pos->z_reset_counter != _altitude_reset_counter) {
_local_position_z_start_of_transition += _local_pos->delta_z; _local_position_z_start_of_transition += _local_pos->delta_z;
_altitude_reset_counter = _local_pos->z_reset_counter; _altitude_reset_counter = _local_pos->z_reset_counter;
if (PX4_ISFINITE(_quadchute_ref_alt)) {
_quadchute_ref_alt += _local_pos->delta_z;
}
} }
} }

2
src/modules/vtol_att_control/vtol_type.h
View File

@ -316,7 +316,7 @@ protected:
hrt_abstime _last_loop_ts = 0; hrt_abstime _last_loop_ts = 0;
float _transition_dt = 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; float _accel_to_pitch_integ = 0;