Implemented VTOL_TAKEOFF and VTOL_LAND commands

msg/vehicle_command.msg

@@ -11,6 +11,8 @@ uint32 VEHICLE_CMD_NAV_LAND = 21			# Land at location |Empty| Empty| Empty| Desi
 uint32 VEHICLE_CMD_NAV_TAKEOFF = 22			# Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| 
 uint32 VEHICLE_CMD_NAV_ROI = 80				# Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| 
 uint32 VEHICLE_CMD_NAV_PATHPLANNING = 81		# Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| 
+uint32 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84		# Takeoff from ground / hand and transition to fixed wing |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|
+uint32 VEHICLE_CMD_NAV_VTOL_LAND = 85			# Transition to MC and land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|
 uint32 VEHICLE_CMD_NAV_GUIDED_LIMITS = 90		# set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| 
 uint32 VEHICLE_CMD_NAV_GUIDED_MASTER = 91		# set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| 
 uint32 VEHICLE_CMD_NAV_GUIDED_ENABLE = 92		# hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| 

src/modules/mavlink/mavlink_mission.cpp

@@ -865,6 +865,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 	// specific item handling
 	switch (mission_item->nav_cmd) {
 	case NAV_CMD_TAKEOFF:
+	case NAV_CMD_VTOL_TAKEOFF:
 		mavlink_mission_item->param1 = mission_item->pitch_min;
 		break;
 

src/modules/navigator/mission.cpp

@@ -61,6 +61,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/vehicle_command.h>
 
 #include "mission.h"
 #include "navigator.h"
@@ -77,6 +78,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_current_onboard_mission_index(-1),
 	_current_offboard_mission_index(-1),
 	_need_takeoff(true),
+	_takeoff_vtol_transition(false),
 	_mission_type(MISSION_TYPE_NONE),
 	_inited(false),
 	_home_inited(false),
@@ -460,7 +462,21 @@ Mission::set_mission_items()
 		}
 
 		/* if we just did a takeoff navigate to the actual waypoint now */
-		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
+		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {			
+
+			/* handle VTOL TAKEOFF command */
+			if(_takeoff_vtol_transition){
+				struct vehicle_command_s cmd = {};
+				cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
+				cmd.param1 = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
+				if (_cmd_pub != nullptr) {
+					orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
+				} else {
+					_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
+				}
+				_takeoff_vtol_transition = false;
+			}
+
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
@@ -647,11 +663,16 @@ Mission::do_need_takeoff()
 		if (_need_takeoff && (
 				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
 				_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+				_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
 				_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 				_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
 				_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
 				_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
 
+			if(_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF){
+				_takeoff_vtol_transition = true;
+			}
+
 			_need_takeoff = false;
 			return true;
 		}
@@ -663,6 +684,18 @@ Mission::do_need_takeoff()
 bool
 Mission::do_need_move_to_land()
 {
+	if(_mission_item.nav_cmd == NAV_CMD_VTOL_LAND){
+		struct vehicle_command_s cmd = {};
+		cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
+		cmd.param1 = vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+		if (_cmd_pub != nullptr) {
+			orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
+		} else {
+			_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
+		}
+		_mission_item.nav_cmd = NAV_CMD_LAND;
+	}
+
 	if (_navigator->get_vstatus()->is_rotary_wing && _mission_item.nav_cmd == NAV_CMD_LAND) {
 
 		float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,

src/modules/navigator/mission.h

@@ -225,6 +225,7 @@ private:
 	int _current_onboard_mission_index;
 	int _current_offboard_mission_index;
 	bool _need_takeoff;					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
+	bool _takeoff_vtol_transition;		/**< if true, a vtol transition will be performed after takeoff */
 
 	enum {
 		MISSION_TYPE_NONE,

src/modules/navigator/mission_feasibility_checker.cpp

@@ -242,6 +242,8 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 			missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
 			missionitem.nav_cmd != NAV_CMD_LAND &&
 			missionitem.nav_cmd != NAV_CMD_TAKEOFF &&
+			missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
+			missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
 			missionitem.nav_cmd != NAV_CMD_PATHPLANNING &&
 			missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
 			missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&

src/modules/navigator/navigation.h

@@ -63,6 +63,8 @@ enum NAV_CMD {
 	NAV_CMD_PATHPLANNING = 81,
 	NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder
 	NAV_CMD_GOTO_TAREGT = 195,
+	NAV_CMD_VTOL_TAKEOFF = 84,
+	NAV_CMD_VTOL_LAND = 85,
 	NAV_CMD_DO_JUMP = 177,
 	NAV_CMD_DO_CHANGE_SPEED = 178,
 	NAV_CMD_DO_SET_SERVO=183,