From f8dc915789063bd18fa14c929e98dff269fdbce7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 9 Sep 2021 22:02:05 +0200 Subject: [PATCH] vtol_att_control: switch to events --- src/modules/vtol_att_control/standard.cpp | 2 +- src/modules/vtol_att_control/tailsitter.cpp | 2 +- .../vtol_att_control_main.cpp | 54 +++++++++++++++++-- .../vtol_att_control/vtol_att_control_main.h | 12 ++++- src/modules/vtol_att_control/vtol_type.cpp | 12 ++--- 5 files changed, 70 insertions(+), 12 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 41e5a4a432..8bcd4b7727 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -277,7 +277,7 @@ void Standard::update_transition_state() if (_params->front_trans_timeout > FLT_EPSILON) { if (time_since_trans_start > _params->front_trans_timeout) { // transition timeout occured, abort transition - _attc->quadchute("Transition timeout"); + _attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout); } } diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 1138c8c776..e76b925f86 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -244,7 +244,7 @@ void Tailsitter::update_transition_state() if (_params->front_trans_timeout > FLT_EPSILON) { if (time_since_trans_start > _params->front_trans_timeout) { // transition timeout occured, abort transition - _attc->quadchute("Transition timeout"); + _attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout); } } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index d864971712..9d8c052aa2 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -47,6 +47,7 @@ * */ #include "vtol_att_control_main.h" +#include #include #include @@ -200,10 +201,52 @@ VtolAttitudeControl::is_fixed_wing_requested() } void -VtolAttitudeControl::quadchute(const char *reason) +VtolAttitudeControl::quadchute(QuadchuteReason reason) { if (!_vtol_vehicle_status.vtol_transition_failsafe) { - mavlink_log_critical(&_mavlink_log_pub, "Quadchute: %s", reason); + switch (reason) { + case QuadchuteReason::TransitionTimeout: + mavlink_log_critical(&_mavlink_log_pub, "Quadchute: timeout\t"); + events::send(events::ID("vtol_att_ctrl_quadchute_tout"), events::Log::Critical, "Quadchute triggered, due to timeout"); + break; + + case QuadchuteReason::ExternalCommand: + mavlink_log_critical(&_mavlink_log_pub, "Quadchute: external command\t"); + events::send(events::ID("vtol_att_ctrl_quadchute_ext_cmd"), events::Log::Critical, + "Quadchute triggered, due to external command"); + break; + + case QuadchuteReason::MinimumAltBreached: + mavlink_log_critical(&_mavlink_log_pub, "Quadchute: minimum altitude breached\t"); + events::send(events::ID("vtol_att_ctrl_quadchute_min_alt"), events::Log::Critical, + "Quadchute triggered, due to minimum altitude breach"); + break; + + case QuadchuteReason::LossOfAlt: + mavlink_log_critical(&_mavlink_log_pub, "Quadchute: loss of altitude\t"); + events::send(events::ID("vtol_att_ctrl_quadchute_alt_loss"), events::Log::Critical, + "Quadchute 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, + "Quadchute triggered, due to large altitude error"); + break; + + case QuadchuteReason::MaximumPitchExceeded: + mavlink_log_critical(&_mavlink_log_pub, "Quadchute: maximum pitch exceeded\t"); + events::send(events::ID("vtol_att_ctrl_quadchute_max_pitch"), events::Log::Critical, + "Quadchute triggered, due to maximum pitch angle exceeded"); + break; + + case QuadchuteReason::MaximumRollExceeded: + mavlink_log_critical(&_mavlink_log_pub, "Quadchute: maximum roll exceeded\t"); + events::send(events::ID("vtol_att_ctrl_quadchute_max_roll"), events::Log::Critical, + "Quadchute triggered, due to maximum roll angle exceeded"); + break; + } + _vtol_vehicle_status.vtol_transition_failsafe = true; } } @@ -258,7 +301,12 @@ VtolAttitudeControl::parameters_update() if (_params.front_trans_time_openloop < _params.front_trans_time_min * 1.1f) { _params.front_trans_time_openloop = _params.front_trans_time_min * 1.1f; param_set_no_notification(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop); - mavlink_log_critical(&_mavlink_log_pub, "OL transition time set larger than min transition time"); + mavlink_log_critical(&_mavlink_log_pub, "OL transition time set larger than min transition time\t"); + /* EVENT + * @description VT_F_TR_OL_TM set to {1:.1}. + */ + events::send(events::ID("vtol_att_ctrl_ol_trans_too_large"), events::Log::Warning, + "Open loop transition time set larger than minimum transition time", _params.front_trans_time_openloop); } param_get(_params_handles.front_trans_duration, &_params.front_trans_duration); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 592c96dc7f..83ea24fc83 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -92,6 +92,16 @@ class VtolAttitudeControl : public ModuleBase, public px4:: { public: + enum class QuadchuteReason { + TransitionTimeout = 0, + ExternalCommand, + MinimumAltBreached, + LossOfAlt, + LargeAltError, + MaximumPitchExceeded, + MaximumRollExceeded, + }; + VtolAttitudeControl(); ~VtolAttitudeControl() override; @@ -107,7 +117,7 @@ public: bool init(); bool is_fixed_wing_requested(); - void quadchute(const char *reason); + void quadchute(QuadchuteReason reason); int get_transition_command() {return _transition_command;} bool get_immediate_transition() {return _immediate_transition;} void reset_immediate_transition() {_immediate_transition = false;} diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 2dc71f4db8..f4570da9d7 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -235,7 +235,7 @@ void VtolType::check_quadchute_condition() { if (_attc->get_transition_command() == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC && _attc->get_immediate_transition() && !_quadchute_command_treated) { - _attc->quadchute("External command"); + _attc->quadchute(VtolAttitudeControl::QuadchuteReason::ExternalCommand); _quadchute_command_treated = true; _attc->reset_immediate_transition(); @@ -256,7 +256,7 @@ void VtolType::check_quadchute_condition() if (_params->fw_min_alt > FLT_EPSILON) { if (-(_local_pos->z) < _params->fw_min_alt) { - _attc->quadchute("Minimum altitude breached"); + _attc->quadchute(VtolAttitudeControl::QuadchuteReason::MinimumAltBreached); } } @@ -274,7 +274,7 @@ void VtolType::check_quadchute_condition() (_ra_hrate < -1.0f) && (_ra_hrate_sp > 1.0f)) { - _attc->quadchute("Loss of altitude"); + _attc->quadchute(VtolAttitudeControl::QuadchuteReason::LossOfAlt); } } else { @@ -282,7 +282,7 @@ void VtolType::check_quadchute_condition() const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f); if (height_error && height_rate_error) { - _attc->quadchute("Large altitude error"); + _attc->quadchute(VtolAttitudeControl::QuadchuteReason::LargeAltError); } } } @@ -291,7 +291,7 @@ void VtolType::check_quadchute_condition() if (_params->fw_qc_max_pitch > 0) { if (fabsf(euler.theta()) > fabsf(math::radians(_params->fw_qc_max_pitch))) { - _attc->quadchute("Maximum pitch angle exceeded"); + _attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumPitchExceeded); } } @@ -299,7 +299,7 @@ void VtolType::check_quadchute_condition() if (_params->fw_qc_max_roll > 0) { if (fabsf(euler.phi()) > fabsf(math::radians(_params->fw_qc_max_roll))) { - _attc->quadchute("Maximum roll angle exceeded"); + _attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumRollExceeded); } } }