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

VTOL: Quad-chute: rework loss of altitude condition 

Previously the condition was based on hard coded height rate estimate and
setpoint values and an altitude error threshold. That showed to be leading
to false positives when the vehicle doesn't tightly follow the altitdue
ramp given by TECS to achieve a new altitude setpoint, and has become
completely infeasibly with the latest TECS rework that leads to non-ramped
altitude setpoint changes in the tecs_status message.
The new check no longer checks the altitude error but only the height rate
instead. It begins to integrate the height rate error once it detects an
uncommanded descend condition (height rate negative while setpoint is
positive). Integral threshold can be tuned by user (VT_QC_HR_ERROR_I).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-01-12 17:07:26 +01:00
parent 36dc75bedf
commit f3cdf70732
4 changed files with 40 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -213,10 +213,10 @@ VtolAttitudeControl::quadchute(QuadchuteReason reason)
"Quad-chute triggered due to minimum altitude breach");
break;
case QuadchuteReason::LossOfAlt:
mavlink_log_critical(&_mavlink_log_pub, "Quadchute: loss of altitude\t");
case QuadchuteReason::UncommandedDescent:
mavlink_log_critical(&_mavlink_log_pub, "Quadchute: Uncommanded descent detected\t");
events::send(events::ID("vtol_att_ctrl_quadchute_alt_loss"), events::Log::Critical,
"Quad-chute triggered due to loss of altitude");
"Quad-chute triggered due to uncommanded descent detection");
break;
case QuadchuteReason::TransitionAltitudeLoss:

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

@ -212,10 +212,14 @@ PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
/**
* Adaptive QuadChute
* Quad-chute uncommanded descent threshold
*
* Maximum negative altitude error for fixed wing flight. If the altitude drops below this value below the altitude setpoint
* the vehicle will transition back to MC mode and enter failsafe RTL.
* 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.
*
* Set to 0 do disable this threshold.
*
* @unit m
* @min 0.0
@ -224,7 +228,7 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
* @decimal 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f);
PARAM_DEFINE_FLOAT(VT_QC_HR_ERROR_I, 0.0f);
/**
* Quad-chute transition altitude loss threshold

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

@ -257,17 +257,28 @@ bool VtolType::isMinAltBreached()
return false;
}
bool VtolType::largeAltitudeLoss()
bool VtolType::isUncommandedDescent()
{
// adaptive quadchute
if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled && _tecs_running) {
if (_param_vt_qc_hr_error_i.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled
&& hrt_elapsed_time(&_tecs_status->timestamp) < 1_s) {
// are we dropping while requesting significant ascend?
if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _param_vt_fw_alt_err.get()) &&
(_ra_hrate < -1.0f) &&
(_ra_hrate_sp > 1.0f)) {
return true;
// TODO if TECS publishes local_position_setpoint dependency on tecs_status can be dropped here
if (_tecs_status->height_rate < -FLT_EPSILON && _tecs_status->height_rate_setpoint > FLT_EPSILON) {
// vehicle is currently in uncommended descend, start integrating error
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());
}
return false;
@ -340,8 +351,8 @@ QuadchuteReason VtolType::getQuadchuteReason()
return QuadchuteReason::MinimumAltBreached;
}
if (largeAltitudeLoss()) {
return QuadchuteReason::LossOfAlt;
if (isUncommandedDescent()) {
return QuadchuteReason::UncommandedDescent;
}
if (isFrontTransitionAltitudeLoss()) {
@ -363,20 +374,6 @@ QuadchuteReason VtolType::getQuadchuteReason()
return QuadchuteReason::None;
}
void VtolType::filterTecsHeightRates()
{
if (_tecs_running) {
// 1 second rolling average
_ra_hrate = (49 * _ra_hrate + _tecs_status->height_rate) / 50;
_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50;
} else {
// reset the filtered height rate and heigh rate setpoint if TECS is not running
_ra_hrate = 0.0f;
_ra_hrate_sp = 0.0f;
}
}
void VtolType::handleSpecialExternalCommandQuadchute()
{
if (_attc->get_transition_command() == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC && _attc->get_immediate_transition()
@ -392,8 +389,6 @@ void VtolType::handleSpecialExternalCommandQuadchute()
void VtolType::check_quadchute_condition()
{
filterTecsHeightRates();
handleSpecialExternalCommandQuadchute();
if (isQuadchuteEnabled()) {

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

@ -89,7 +89,7 @@ enum class QuadchuteReason {
TransitionTimeout,
ExternalCommand,
MinimumAltBreached,
LossOfAlt,
UncommandedDescent,
TransitionAltitudeLoss,
MaximumPitchExceeded,
MaximumRollExceeded,
@ -165,12 +165,11 @@ public:
bool isMinAltBreached();
/**
* @brief Indicates if the vehicle has an altitude error larger than VT_FW_ALT_ERR and is losing altitude quickly.
* This only applies when TECS is running.
* @brief Indicates if conditions are met for uncommanded-descent quad-chute.
*
* @return true if error larger than threshold
* @return true if integrated height rate error larger than threshold
*/
bool largeAltitudeLoss();
bool isUncommandedDescent();
/**
* @brief Indicates if there is an altitude loss higher than specified threshold during a VTOL transition to FW
@ -200,11 +199,6 @@ public:
*/
bool isFrontTransitionTimeout();
/**
* @brief Applied a first order low pass filte to TECS height rate and heigh rate setpoint.
*/
void filterTecsHeightRates();
/**
* @brief Special handling of QuadchuteReason::ReasonExternal
*/
@ -290,8 +284,8 @@ 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 _ra_hrate = 0.0f; // rolling average on height rate for quadchute condition
float _ra_hrate_sp = 0.0f; // rolling average on height rate setpoint for quadchute condition
float _height_rate_error_integral{0.f};
hrt_abstime _trans_finished_ts = 0;
hrt_abstime _transition_start_timestamp{0};
@ -302,6 +296,7 @@ protected:
hrt_abstime _last_loop_ts = 0;
float _transition_dt = 0;
hrt_abstime _last_loop_quadchute_timestamp = 0;
float _accel_to_pitch_integ = 0;
@ -319,7 +314,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_FW_ALT_ERR>) _param_vt_fw_alt_err,
(ParamFloat<px4::params::VT_QC_HR_ERROR_I>) _param_vt_qc_hr_error_i,
(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,