Front transition timeout

src/modules/vtol_att_control/standard.cpp

@@ -35,7 +35,8 @@
  * @file standard.cpp
  *
  * @author Simon Wilks		<simon@uaventure.com>
- * @author Roman Bapst 		<bapstroman@gmail.com>
+ * @author Roman Bapst		<bapstroman@gmail.com>
+ * @author Sander Smeets	<sander@droneslab.com>
  *
 */
 
@@ -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) {

src/modules/vtol_att_control/standard.h

@@ -38,6 +38,7 @@
 *
 * @author Simon Wilks 		<simon@uaventure.com>
 * @author Roman Bapst 		<bapstroman@gmail.com>
+* @author Sander Smeets 	<sander@droneslab.com>
 *
 */
 
@@ -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 {

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -41,10 +41,12 @@
  * @author Roman Bapst 		<bapstr@ethz.ch>
  * @author Lorenz Meier 	<lm@inf.ethz.ch>
  * @author Thomas Gubler	<thomasgubler@gmail.com>
- * @author David Vorsin     <davidvorsin@gmail.com>
+ * @author David Vorsin		<davidvorsin@gmail.com>
+ * @author Sander Smeets	<sander@droneslab.com>
  *
  */
 #include "vtol_att_control_main.h"
+#include <mavlink/mavlink_log.h>
 
 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;

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -41,7 +41,8 @@
  * @author Roman Bapst 		<bapstr@ethz.ch>
  * @author Lorenz Meier 	<lm@inf.ethz.ch>
  * @author Thomas Gubler	<thomasgubler@gmail.com>
- * @author David Vorsin     <davidvorsin@gmail.com>
+ * @author David Vorsin		<davidvorsin@gmail.com>
+ * @author Sander Smeets	<sander@droneslab.com>
  *
  */
 #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

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);