diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 93a8cfbdbe3da12ac46716462894023b1a1f7771..694317785dc6cf7ae3c3cc02181b9d8d1be7b1e3 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -285,7 +285,7 @@ void Standard::update_transition_state() if (_params_standard.front_trans_timeout > FLT_EPSILON) { if ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1000000.0f)) { // transition timeout occured, abort transition - _attc->abort_front_transition(); + _attc->abort_front_transition("Transition timeout"); } } 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 555563fbfe322768e7884dd4ad48031a7134a830..8f623461ec53324176bc3ddc2b2ef6d1cda708d7 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -510,10 +510,10 @@ VtolAttitudeControl::is_fixed_wing_requested() * Abort front transition */ void -VtolAttitudeControl::abort_front_transition() +VtolAttitudeControl::abort_front_transition(char *reason) { - if (!_abort_front_transition) { - mavlink_log_critical(&_mavlink_log_pub, "Transition timeout or FW min alt occured, aborting"); + if(!_abort_front_transition) { + mavlink_log_critical(&_mavlink_log_pub, "Abort: %s", reason); _abort_front_transition = true; _vtol_vehicle_status.vtol_transition_failsafe = true; } 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 3412a53990664ad89375dbb1f9dbde09bac56f11..762d6bd2b2b13c1cd445b4ab33a336389e549224 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -110,7 +110,7 @@ public: int start(); /* start the task and return OK on success */ bool is_fixed_wing_requested(); - void abort_front_transition(); + void abort_front_transition(char *reason); struct vehicle_attitude_s *get_att() {return &_v_att;} struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index ae3ed92ef54ab7fe74a7526108d49275802e5ae5..71c7432bdeffd8b1f3c526930c552b7fbf697718 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -180,27 +180,25 @@ void VtolType::update_fw_state() waiting_on_tecs(); } - // quadchute - if (_params->fw_min_alt > FLT_EPSILON && _armed->armed) { - if (-(_local_pos->z) < _params->fw_min_alt) { - _attc->abort_front_transition(); - } - } - + check_quadchute_condition(); } void VtolType::update_transition_state() { - // quadchute - if (_params->fw_min_alt > FLT_EPSILON && _armed->armed) { - if (-(_local_pos->z) < _params->fw_min_alt) { - _attc->abort_front_transition(); - } - } - + check_quadchute_condition(); } bool VtolType::can_transition_on_ground() { return !_armed->armed || _land_detected->landed; } + +void VtolType::check_quadchute_condition() +{ + // fixed-wing minimum altitude + if(_params->fw_min_alt > FLT_EPSILON && _armed->armed){ + if(-(_local_pos->z) < _params->fw_min_alt){ + _attc->abort_front_transition("Minimum altitude"); + } + } +} diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 4570e39d8e34b14283815e64162d5b54630210e2..96f11fd51ecba3ca042cc19def150e57da8ea2a5 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -123,6 +123,11 @@ public: */ virtual void waiting_on_tecs() {}; + /** + * Checks for fixed-wing failsafe condition and issues abort request if needed. + */ + void check_quadchute_condition(); + /** * Returns true if we're allowed to do a mode transition on the ground. */