implemented MAVLink command for VTOL transitions, pulled switch up for each type

msg/vehicle_command.msg

@@ -50,6 +50,7 @@ uint32 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item:
 uint32 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm| 
 uint32 VEHICLE_CMD_START_RX_PAIR = 500			# Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|
 uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003            # Enable or disable on-board camera triggering system
+uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    # Command VTOL transition
 uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
 

msg/vehicle_status.msg

@@ -79,6 +79,13 @@ uint8 VEHICLE_TYPE_VTOL_HEXAROTOR = 21 		# Vtol with six engines
 uint8 VEHICLE_TYPE_VTOL_OCTOROTOR = 22		# Vtol with eight engines
 uint8 VEHICLE_TYPE_ENUM_END = 23
 
+# VEHICLE_VTOL_STATE, should match 1:1 MAVLinks's MAV_VTOL_STATE
+uint8 VEHICLE_VTOL_STATE_UNDEFINED = 0
+uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1
+uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2
+uint8 VEHICLE_VTOL_STATE_MC = 3
+uint8 VEHICLE_VTOL_STATE_FW = 4
+
 uint8 VEHICLE_BATTERY_WARNING_NONE = 0	    # no battery low voltage warning active
 uint8 VEHICLE_BATTERY_WARNING_LOW = 1	    # warning of low voltage
 uint8 VEHICLE_BATTERY_WARNING_CRITICAL = 2  # alerting of critical voltage

src/modules/commander/commander.cpp

@@ -794,6 +794,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
 	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
 	case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL:
+	case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
 		/* ignore commands that handled in low prio loop */
 		break;
 

src/modules/vtol_att_control/standard.cpp

@@ -105,7 +105,7 @@ void Standard::update_vtol_state()
 	 * For the back transition the pusher motor is immediately stopped and rotors reactivated.
  	 */
 
-	if (_manual_control_sp->aux1 < 0.0f) {
+	if (!is_fixed_wing_requested()) {
 		// the transition to fw mode switch is off
 		if (_vtol_schedule.flight_mode == MC_MODE) {
 			// in mc mode

src/modules/vtol_att_control/tailsitter.cpp

@@ -58,9 +58,9 @@ Tailsitter::~Tailsitter()
 void Tailsitter::update_vtol_state()
 {
 	// simply switch between the two modes
-	if (_manual_control_sp->aux1 < 0.0f) {
+	if (!is_fixed_wing_requested()) {
 		_vtol_mode = ROTARY_WING;
-	} else if (_manual_control_sp->aux1 > 0.0f) {
+	} else {
 		_vtol_mode = FIXED_WING;
 	}
 }

src/modules/vtol_att_control/tiltrotor.cpp

@@ -158,7 +158,7 @@ void Tiltrotor::update_vtol_state()
 	 * forward completely. For the backtransition the motors simply rotate back.
  	*/
 
-	if (_manual_control_sp->aux1 < 0.0f) {
+	if (!is_fixed_wing_requested()) {
 
 		// plane is in multicopter mode
 		switch(_vtol_schedule.flight_mode) {

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -69,6 +69,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_local_pos_sub(-1),
 	_airspeed_sub(-1),
 	_battery_status_sub(-1),
+	_vehicle_cmd_sub(-1),
 
 	//init publication handlers
 	_actuators_0_pub(nullptr),
@@ -95,6 +96,8 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	memset(&_local_pos,0,sizeof(_local_pos));
 	memset(&_airspeed,0,sizeof(_airspeed));
 	memset(&_batt_status,0,sizeof(_batt_status));
+	memset(&_vehicle_cmd,0, sizeof(_vehicle_cmd));
+	memset(&_vehicle_transition_cmd,0, sizeof(_vehicle_cmd));
 
 	_params.idle_pwm_mc = PWM_LOWEST_MIN;
 	_params.vtol_motor_count = 0;
@@ -312,6 +315,22 @@ VtolAttitudeControl::vehicle_local_pos_poll()
 
 }
 
+/**
+* Check for command updates.
+*/
+void
+VtolAttitudeControl::vehicle_cmd_poll() {
+	bool updated;
+	orb_check(_vehicle_cmd_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub , &_vehicle_cmd);
+		if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
+			orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub , &_vehicle_transition_cmd);
+		}
+	}
+}
+
 /**
 * Update parameters.
 */
@@ -412,6 +431,7 @@ void VtolAttitudeControl::task_main()
 	_local_pos_sub         = orb_subscribe(ORB_ID(vehicle_local_position));
 	_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));
 	_battery_status_sub	   = orb_subscribe(ORB_ID(battery_status));
+	_vehicle_cmd_sub	   = orb_subscribe(ORB_ID(vehicle_command));
 
 	_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
 	_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
@@ -483,6 +503,7 @@ void VtolAttitudeControl::task_main()
 		vehicle_local_pos_poll();			// Check for new sensor values
 		vehicle_airspeed_poll();
 		vehicle_battery_poll();
+		vehicle_cmd_poll();
 
 		// update the vtol state machine which decides which mode we are in
 		_vtol_type->update_vtol_state();

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -73,6 +73,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_command.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -115,6 +116,7 @@ public:
 	struct vehicle_local_position_s* 	get_local_pos () {return &_local_pos;}
 	struct airspeed_s* 					get_airspeed () {return &_airspeed;}
 	struct battery_status_s* 			get_batt_status () {return &_batt_status;}
+	struct vehicle_command_s*			get_vehicle_transition_cmd () {return &_vehicle_transition_cmd;}
 
 	struct Params* 						get_params () {return &_params;}
 
@@ -136,6 +138,7 @@ private:
 	int 	_local_pos_sub;			// sensor subscription
 	int 	_airspeed_sub;			// airspeed subscription
 	int 	_battery_status_sub;	// battery status subscription
+	int 	_vehicle_cmd_sub;
 
 	int 	_actuator_inputs_mc;	//topic on which the mc_att_controller publishes actuator inputs
 	int 	_actuator_inputs_fw;	//topic on which the fw_att_controller publishes actuator inputs
@@ -162,6 +165,8 @@ private:
 	struct vehicle_local_position_s		_local_pos;
 	struct airspeed_s 					_airspeed;			// airspeed
 	struct battery_status_s 			_batt_status; 		// battery status
+	struct vehicle_command_s			_vehicle_cmd;
+	struct vehicle_command_s			_vehicle_transition_cmd; // stores transition commands only
 
 	Params _params;	// struct holding the parameters
 
@@ -206,6 +211,7 @@ private:
 	void 		vehicle_local_pos_poll();		// Check for changes in sensor values
 	void 		vehicle_airspeed_poll();		// Check for changes in airspeed
 	void 		vehicle_battery_poll();			// Check for battery updates
+	void		vehicle_cmd_poll();
 	void 		parameters_update_poll();		//Check if parameters have changed
 	int 		parameters_update();			//Update local paraemter cache
 	void 		fill_mc_att_rates_sp();

src/modules/vtol_att_control/vtol_type.cpp

@@ -63,6 +63,7 @@ _vtol_mode(ROTARY_WING)
 	_local_pos = _attc->get_local_pos();
 	_airspeed = _attc->get_airspeed();
 	_batt_status = _attc->get_batt_status();
+	_vehicle_transition_cmd = _attc->get_vehicle_transition_cmd();
 	_params = _attc->get_params();
 
 	flag_idle_mc = true;
@@ -130,4 +131,17 @@ void VtolType::set_idle_fw()
 	if (ret != OK) {errx(ret, "failed setting min values");}
 
 	close(fd);
-}
+}
+
+/*
+ * Return true if fixed-wing mode is requested.
+ * Either via switch or via command.
+ */
+bool VtolType::is_fixed_wing_requested()
+{
+	bool to_fw = _manual_control_sp->aux1 > 0.0f;
+ 	if (_v_control_mode->flag_control_offboard_enabled) {
+ 		to_fw = fabsf(_vehicle_transition_cmd->param1 - vehicle_status_s::VEHICLE_VTOL_STATE_FW) < FLT_EPSILON;
+ 	}
+ 	return to_fw;
+}

src/modules/vtol_att_control/vtol_type.h

@@ -87,6 +87,8 @@ public:
 
 	mode get_mode () {return _vtol_mode;};
 
+	bool is_fixed_wing_requested();
+
 protected:
 	VtolAttitudeControl *_attc;
 	mode _vtol_mode;
@@ -107,6 +109,7 @@ protected:
 	struct vehicle_local_position_s		*_local_pos;
 	struct airspeed_s 					*_airspeed;			// airspeed
 	struct battery_status_s 			*_batt_status; 		// battery status
+	struct vehicle_command_s			*_vehicle_transition_cmd;
 
 	struct Params 						*_params;