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

VTOL: introduce new quad-chute check for altitude loss during front transition 

By default the threshold is set to 10m.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-01-12 11:59:23 +01:00
parent 98705ced2f
commit 36dc75bedf
7 changed files with 56 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/vtol_att_control/standard.cpp
View File

@ -89,7 +89,7 @@ void Standard::update_vtol_state()
} else if (_vtol_mode == vtol_mode::FW_MODE) {
// Regular backtransition
resetTransitionTimer();
resetTransitionStates();
_vtol_mode = vtol_mode::TRANSITION_TO_MC;
_reverse_output = _param_vt_b_rev_out.get();
@ -126,7 +126,7 @@ void Standard::update_vtol_state()
// start transition to fw mode
/* NOTE: The failsafe transition to fixed-wing was removed because it can result in an
* unsafe flying state. */
resetTransitionTimer();
resetTransitionStates();
_vtol_mode = vtol_mode::TRANSITION_TO_FW;
} else if (_vtol_mode == vtol_mode::FW_MODE) {

4
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -82,7 +82,7 @@ void Tailsitter::update_vtol_state()
break;
case vtol_mode::FW_MODE:
resetTransitionTimer();
resetTransitionStates();
_vtol_mode = vtol_mode::TRANSITION_BACK;
break;
@ -107,7 +107,7 @@ void Tailsitter::update_vtol_state()
case vtol_mode::MC_MODE:
// initialise a front transition
_vtol_mode = vtol_mode::TRANSITION_FRONT_P1;
resetTransitionTimer();
resetTransitionStates();
break;
case vtol_mode::FW_MODE:

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

@ -83,7 +83,7 @@ void Tiltrotor::update_vtol_state()
break;
case vtol_mode::FW_MODE:
resetTransitionTimer();
resetTransitionStates();
_vtol_mode = vtol_mode::TRANSITION_BACK;
break;
@ -126,7 +126,7 @@ void Tiltrotor::update_vtol_state()
switch (_vtol_mode) {
case vtol_mode::MC_MODE:
// initialise a front transition
resetTransitionTimer();
resetTransitionStates();
_vtol_mode = vtol_mode::TRANSITION_FRONT_P1;
break;
@ -154,7 +154,7 @@ void Tiltrotor::update_vtol_state()
if (transition_to_p2) {
_vtol_mode = vtol_mode::TRANSITION_FRONT_P2;
resetTransitionTimer();
resetTransitionStates();
}
break;

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

@ -219,10 +219,10 @@ VtolAttitudeControl::quadchute(QuadchuteReason reason)
"Quad-chute triggered due to loss of altitude");
break;
case QuadchuteReason::LargeAltError:
mavlink_log_critical(&_mavlink_log_pub, "Quadchute: large altitude error\t");
events::send(events::ID("vtol_att_ctrl_quadchute_alt_err"), events::Log::Critical,
"Quad-chute triggered due to large altitude error");
case QuadchuteReason::TransitionAltitudeLoss:
mavlink_log_critical(&_mavlink_log_pub, "Quadchute: loss of altitude during transition\t");
events::send(events::ID("vtol_att_ctrl_quadchute_trans_alt_err"), events::Log::Critical,
"Quad-chute triggered due to loss of altitude during transition");
break;
case QuadchuteReason::MaximumPitchExceeded:

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

@ -226,6 +226,24 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
*/
PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f);
/**
* Quad-chute transition altitude loss threshold
*
* Altitude loss threshold for quad-chute triggering during VTOL transition to fixed-wing flight.
* If the current altitude is more than this value below the altitude at the beginning of the
* transition, it will instantly switch back to MC mode and execute behavior defined in COM_QC_ACT.
*
* Set to 0 do disable this threshold.
*
* @unit m
* @min 0
* @max 50
* @increment 1
* @decimal 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_QC_T_ALT_LOSS, 10.0f);
/**
* Quad-chute max pitch threshold
*

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

@ -212,6 +212,13 @@ bool VtolType::can_transition_on_ground()
return !_v_control_mode->flag_armed || _land_detected->landed;
}
void VtolType::resetTransitionStates()
{
_transition_start_timestamp = hrt_absolute_time();
_time_since_trans_start = 0.f;
_local_position_z_start_of_transition = _local_pos->z;
}
bool VtolType::isQuadchuteEnabled()
{
float dist_to_ground = 0.f;
@ -266,21 +273,23 @@ bool VtolType::largeAltitudeLoss()
return false;
}
bool VtolType::largeAltitudeError()
bool VtolType::isFrontTransitionAltitudeLoss()
{
// adaptive quadchute
if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled && !_tecs_running) {
bool result = false;
if (_param_vt_qc_t_alt_loss.get() > FLT_EPSILON && _common_vtol_mode == mode::TRANSITION_TO_FW && _local_pos->z_valid) {
const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _param_vt_fw_alt_err.get());
const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f);
if (_local_pos->z <= FLT_EPSILON) {
// vehilce is above home
result = _local_pos->z - _local_position_z_start_of_transition > _param_vt_qc_t_alt_loss.get();
if (height_error && height_rate_error) {
return true;
} else {
// vehilce is below home
result = _local_position_z_start_of_transition - _local_pos->z > _param_vt_qc_t_alt_loss.get();
}
}
return false;
return result;
}
bool VtolType::isPitchExceeded()
@ -335,8 +344,8 @@ QuadchuteReason VtolType::getQuadchuteReason()
return QuadchuteReason::LossOfAlt;
}
if (largeAltitudeError()) {
return QuadchuteReason::LargeAltError;
if (isFrontTransitionAltitudeLoss()) {
return QuadchuteReason::TransitionAltitudeLoss;
}
if (isPitchExceeded()) {

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

@ -90,7 +90,7 @@ enum class QuadchuteReason {
ExternalCommand,
MinimumAltBreached,
LossOfAlt,
LargeAltError,
TransitionAltitudeLoss,
MaximumPitchExceeded,
MaximumRollExceeded,
};
@ -173,11 +173,11 @@ public:
bool largeAltitudeLoss();
/**
* @brief Indicates if the vehicle has an altitude error larger than VT_FW_ALT_ERR. This only applied when TECS is not running.
* @brief Indicates if there is an altitude loss higher than specified threshold during a VTOL transition to FW
*
* @return true if error larger than threshold
* @return true if error larger than threshold
*/
bool largeAltitudeError();
bool isFrontTransitionAltitudeLoss();
/**
* @brief Indicates if the absolute value of the vehicle pitch angle exceeds the threshold defined by VT_FW_QC_P
@ -252,11 +252,7 @@ public:
* @brief Resets the transition timer states.
*
*/
void resetTransitionTimer()
{
_transition_start_timestamp = hrt_absolute_time();
_time_since_trans_start = 0.f;
}
void resetTransitionStates();
protected:
VtolAttitudeControl *_attc;
@ -318,12 +314,15 @@ protected:
float _dt{0.0025f}; // time step [s]
float _local_position_z_start_of_transition{0.f}; // altitude at start of transition
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,
(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,
(ParamInt<px4::params::VT_FW_QC_HMAX>) _param_quadchute_max_height,
(ParamFloat<px4::params::VT_F_TR_OL_TM>) _param_vt_f_tr_ol_tm,
(ParamFloat<px4::params::VT_TRANS_MIN_TM>) _param_vt_trans_min_tm,