diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index c0d34c0384..27ed3359f5 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -35,7 +35,8 @@ * @file standard.cpp * * @author Simon Wilks - * @author Roman Bapst + * @author Roman Bapst + * @author Sander Smeets * */ @@ -61,6 +62,7 @@ Standard::Standard(VtolAttitudeControl *attc) : _params_handles_standard.pusher_trans = param_find("VT_TRANS_THR"); _params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND"); _params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_standard.front_trans_timeout = param_find("VT_TRANS_TIMEOUT"); } Standard::~Standard() @@ -84,7 +86,7 @@ Standard::parameters_update() param_get(_params_handles_standard.pusher_trans, &v); _params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f); - /* airspeed at which it we should switch to fw mode */ + /* airspeed at which we should switch to fw mode */ param_get(_params_handles_standard.airspeed_trans, &v); _params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f); @@ -94,6 +96,9 @@ Standard::parameters_update() _airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend; + /* timeout for transition to fw mode */ + param_get(_params_handles_standard.front_trans_timeout, &_params_standard.front_trans_timeout); + return OK; } @@ -221,6 +226,14 @@ void Standard::update_transition_state() _mc_throttle_weight = 1.0f; } + // check front transition timeout + 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(); + } + } + } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // continually increase mc attitude control as we transition back to mc mode if (_params_standard.back_trans_dur > 0.0f) { diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index ed37ccc398..19341c1366 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -38,6 +38,7 @@ * * @author Simon Wilks * @author Roman Bapst +* @author Sander Smeets * */ @@ -69,6 +70,7 @@ private: float pusher_trans; float airspeed_blend; float airspeed_trans; + float front_trans_timeout; } _params_standard; struct { @@ -77,6 +79,7 @@ private: param_t pusher_trans; param_t airspeed_blend; param_t airspeed_trans; + param_t front_trans_timeout; } _params_handles_standard; enum vtol_mode { 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 b6b2cdade0..780b8e8cf8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -41,10 +41,12 @@ * @author Roman Bapst * @author Lorenz Meier * @author Thomas Gubler - * @author David Vorsin + * @author David Vorsin + * @author Sander Smeets * */ #include "vtol_att_control_main.h" +#include namespace VTOL_att_control { @@ -58,6 +60,9 @@ VtolAttitudeControl::VtolAttitudeControl() : _task_should_exit(false), _control_task(-1), + // mavlink log + _mavlink_fd(-1), + //init subscription handlers _v_att_sub(-1), _v_att_sp_sub(-1), @@ -427,9 +432,31 @@ VtolAttitudeControl::is_fixed_wing_requested() to_fw = _transition_command == vehicle_status_s::VEHICLE_VTOL_STATE_FW; } + // handle abort request + if(_abort_front_transition) { + if(to_fw) { + to_fw = false; + } else { + // the state changed to mc mode, reset the abort request + _abort_front_transition = false; + } + } + return to_fw; } +/* + * Abort front transition + */ +void +VtolAttitudeControl::abort_front_transition() +{ + if(!_abort_front_transition) { + mavlink_log_critical(_mavlink_fd, "Front transition timeout occured, aborting"); + _abort_front_transition = true; + } +} + /** * Update parameters. */ @@ -530,6 +557,8 @@ void VtolAttitudeControl::task_main() PX4_WARN("started"); fflush(stdout); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); + /* do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint)); @@ -558,6 +587,9 @@ void VtolAttitudeControl::task_main() // make sure we start with idle in mc mode _vtol_type->set_idle_mc(); + hrt_abstime mavlink_open_time = 0; + const hrt_abstime mavlink_open_interval = 500000; + /* wakeup source*/ px4_pollfd_struct_t fds[3] = {}; /*input_mc, input_fw, parameters*/ @@ -595,6 +627,13 @@ void VtolAttitudeControl::task_main() continue; } + if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { + /* try to reopen the mavlink log device with specified interval */ + mavlink_open_time = hrt_abstime() + mavlink_open_interval; + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); + } + + if (fds[2].revents & POLLIN) { //parameters were updated, read them now /* read from param to clear updated flag */ struct parameter_update_s update; 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 0e99d58aab..6cf1926a9c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -41,7 +41,8 @@ * @author Roman Bapst * @author Lorenz Meier * @author Thomas Gubler - * @author David Vorsin + * @author David Vorsin + * @author Sander Smeets * */ #ifndef VTOL_ATT_CONTROL_MAIN_H @@ -106,6 +107,7 @@ public: int start(); /* start the task and return OK on success */ bool is_fixed_wing_requested(); + void abort_front_transition(); struct vehicle_attitude_s *get_att() {return &_v_att;} struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} @@ -133,6 +135,7 @@ private: //******************flags & handlers****************************************************** bool _task_should_exit; int _control_task; //task handle for VTOL attitude controller + int _mavlink_fd; // mavlink log device /* handlers for subscriptions */ int _v_att_sub; //vehicle attitude subscription @@ -201,6 +204,7 @@ private: * for fixed wings we want to have an idle speed of zero since we do not want * to waste energy when gliding. */ int _transition_command; + bool _abort_front_transition; VtolType *_vtol_type; // base class for different vtol types Tiltrotor *_tiltrotor; // tailsitter vtol type diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 49643ee422..db5d8d051d 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -212,3 +212,14 @@ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_OPT_RECOV_EN, 0); + +/** + * Front transition timeout + * + * Time in seconds after which transition will be cancelled. Disabled if set to 0. + * + * @min 0.0 + * @max 30.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TRANS_TIMEOUT, 15.0f);