Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 03:13:44 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

vtol_att_control: switch to events

Browse Source
This commit is contained in:
Beat Küng
2021-09-09 22:02:05 +02:00
committed by Daniel Agar
parent dc87f8a7a4
commit f8dc915789
5 changed files with 70 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/standard.cpp
+1 -1
View File
@@ -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);
}
}
src/modules/vtol_att_control/tailsitter.cpp
+1 -1
View File
@@ -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);
}
}
src/modules/vtol_att_control/vtol_att_control_main.cpp
+51 -3
View File
@@ -47,6 +47,7 @@
*
*/
#include "vtol_att_control_main.h"
#include <px4_platform_common/events.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Publication.hpp>
@@ -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 <param>VT_F_TR_OL_TM</param> set to {1:.1}.
*/
events::send<float>(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);
src/modules/vtol_att_control/vtol_att_control_main.h
+11 -1
View File
@@ -92,6 +92,16 @@ class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, 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;}
src/modules/vtol_att_control/vtol_type.cpp
+6 -6
View File
@@ -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);
}
}
}