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

Refactor uncommanded descend Quad-Chute (#21598) 

* refactored uncommanded descend quadchute
- use fixed altitude error threshold
- compute error relative to higest altitude the vehicle has achieved
since it has flown below the altitude reference (setpoint)

* disabled altitude loss quadchute by default

* altitude loss quadchute: added protection against invalid local z


---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Roman Bapst 2023-05-31 18:57:50 +03:00 committed by Silvan Fuhrer
parent 4b7da42a6e
commit 5aa5fc2da5
3 changed files with 20 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -197,12 +197,12 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
/**
* Quad-chute uncommanded descent threshold
*
* Threshold for integrated height rate error to trigger a uncommanded-descent quad-chute.
* Only checked in altitude-controlled fixed-wing flight.
* Additional conditions that have to be met for uncommanded descent detection are a positive (climbing) height
* rate setpoint and a negative (sinking) current height rate estimate.
* Altitude error threshold for quad-chute triggering during fixed-wing flight.
* The check is only active if altitude is controlled and the vehicle is below the current altitude reference.
* The altitude error is relative to the highest altitude the vehicle has achieved since it has flown below the current
* altitude reference.
*
* Set to 0 do disable this threshold.
* Set to 0 do disable.
*
* @unit m
* @min 0.0
@ -211,7 +211,7 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
* @decimal 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_QC_HR_ERROR_I, 0.0f);
PARAM_DEFINE_FLOAT(VT_QC_ALT_LOSS, 0.0f);
/**
* Quad-chute transition altitude loss threshold

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

@ -272,26 +272,21 @@ bool VtolType::isMinAltBreached()
bool VtolType::isUncommandedDescent()
{
if (_param_vt_qc_hr_error_i.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled
const float current_altitude = -_local_pos->z + _local_pos->ref_alt;
if (_param_vt_qc_alt_loss.get() > FLT_EPSILON && _local_pos->z_valid && _local_pos->z_global
&& _v_control_mode->flag_control_altitude_enabled
&& PX4_ISFINITE(_tecs_status->altitude_reference)
&& (current_altitude < _tecs_status->altitude_reference)
&& hrt_elapsed_time(&_tecs_status->timestamp) < 1_s) {
// TODO if TECS publishes local_position_setpoint dependency on tecs_status can be dropped here
_quadchute_ref_alt = math::min(math::max(_quadchute_ref_alt, current_altitude),
_tecs_status->altitude_reference);
if (_tecs_status->height_rate < -FLT_EPSILON && _tecs_status->height_rate_setpoint > FLT_EPSILON) {
// vehicle is currently in uncommended descend, start integrating error
return (_quadchute_ref_alt - current_altitude) > _param_vt_qc_alt_loss.get();
const hrt_abstime now = hrt_absolute_time();
float dt = static_cast<float>(now - _last_loop_quadchute_timestamp) / 1e6f;
dt = math::constrain(dt, 0.0001f, 0.1f);
_last_loop_quadchute_timestamp = now;
_height_rate_error_integral += (_tecs_status->height_rate_setpoint - _tecs_status->height_rate) * dt;
} else {
_height_rate_error_integral = 0.f; // reset
}
return (_height_rate_error_integral > _param_vt_qc_hr_error_i.get());
} else {
_quadchute_ref_alt = -MAXFLOAT;
}
return false;

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

@ -288,9 +288,6 @@ protected:
float _thrust_transition = 0.0f; // thrust value applied during a front transition (tailsitter & tiltrotor only)
float _last_thr_in_fw_mode = 0.0f;
float _height_rate_error_integral{0.f};
hrt_abstime _trans_finished_ts = 0;
hrt_abstime _transition_start_timestamp{0};
float _time_since_trans_start{0};
@ -300,7 +297,8 @@ protected:
hrt_abstime _last_loop_ts = 0;
float _transition_dt = 0;
hrt_abstime _last_loop_quadchute_timestamp = 0;
float _quadchute_ref_alt{-MAXFLOAT}; // altitude (AMSL) reference to compute quad-chute altitude loss condition
float _accel_to_pitch_integ = 0;
@ -317,7 +315,7 @@ protected:
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,
(ParamFloat<px4::params::VT_QC_HR_ERROR_I>) _param_vt_qc_hr_error_i,
(ParamFloat<px4::params::VT_QC_ALT_LOSS>) _param_vt_qc_alt_loss,
(ParamInt<px4::params::VT_FW_QC_P>) _param_vt_fw_qc_p,
(ParamInt<px4::params::VT_FW_QC_R>) _param_vt_fw_qc_r,
(ParamFloat<px4::params::VT_QC_T_ALT_LOSS>) _param_vt_qc_t_alt_loss,