Vtol takeoff/land handling in work items

src/modules/navigator/mission.cpp

@@ -62,7 +62,6 @@
 #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"
@@ -80,7 +79,6 @@ 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),
@@ -464,27 +462,46 @@ 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;
+			if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
+				new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
+
+			} else {
+				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 */
 			_mission_item.yaw = NAN;
 		}
 
+		if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
+				&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF
+				&& _navigator->get_vstatus()->is_rotary_wing
+				&& !_navigator->get_vstatus()->condition_landed
+				&& has_next_position_item) {
+
+			new_work_item_type = WORK_ITEM_TYPE_ALIGN;
+			set_align_mission_item(&_mission_item, &mission_item_next_position);
+		}
+
+		if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
+				&& _work_item_type == WORK_ITEM_TYPE_ALIGN) {
+
+			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
+			_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
+			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+		}
+
+		if(_mission_item.nav_cmd == NAV_CMD_VTOL_LAND ||
+				(_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get())){
+
+			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
+			_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+			new_work_item_type = WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND;
+		}
+
 		/* move to landing waypoint before descent if necessary */
 		if (do_need_move_to_land() && _work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) {
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
@@ -535,17 +552,7 @@ Mission::set_mission_items()
 				&& has_next_position_item) {
 
 			new_work_item_type = WORK_ITEM_TYPE_ALIGN;
-
+			set_align_mission_item(&_mission_item, &mission_item_next_position);
-			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-			copy_positon_if_valid(_mission_item, pos_sp_triplet->current);
-			_mission_item.autocontinue = true;
-			_mission_item.time_inside = 0;
-			_mission_item.yaw = get_bearing_to_next_waypoint(
-				_navigator->get_global_position()->lat,
-				_navigator->get_global_position()->lon,
-				mission_item_next_position.lat,
-				mission_item_next_position.lon);
-			_mission_item.force_heading = true;
 		}
 
 		/* yaw is aligned now */
@@ -570,7 +577,7 @@ Mission::set_mission_items()
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-			copy_positon_if_valid(_mission_item, pos_sp_triplet->current);
+			copy_positon_if_valid(&_mission_item, &(pos_sp_triplet->current));
 			_mission_item.autocontinue = true;
 			_mission_item.time_inside = 0;
 		}
@@ -629,23 +636,6 @@ Mission::set_mission_items()
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
-void
-Mission::copy_positon_if_valid(struct mission_item_s &mission_item, struct position_setpoint_s &setpoint)
-{
-	if (setpoint.valid) {
-		_mission_item.lat = setpoint.lat;
-		_mission_item.lon = setpoint.lon;
-		_mission_item.altitude = setpoint.alt;
-
-	} else {
-		_mission_item.lat = _navigator->get_global_position()->lat;
-		_mission_item.lon = _navigator->get_global_position()->lon;
-		_mission_item.altitude = _navigator->get_global_position()->alt;
-	}
-
-	_mission_item.altitude_is_relative = false;
-}
-
 bool
 Mission::do_need_takeoff()
 {
@@ -671,10 +661,6 @@ Mission::do_need_takeoff()
 				_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;
 		}
@@ -686,18 +672,6 @@ Mission::do_need_takeoff()
 bool
 Mission::do_need_move_to_land()
 {
-	if(_mission_item.nav_cmd == NAV_CMD_VTOL_LAND || (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get())){
-		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,
@@ -709,6 +683,39 @@ Mission::do_need_move_to_land()
 	return false;
 }
 
+void
+Mission::copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint)
+{
+	if (setpoint->valid) {
+		mission_item->lat = setpoint->lat;
+		mission_item->lon = setpoint->lon;
+		mission_item->altitude = setpoint->alt;
+
+	} else {
+		mission_item->lat = _navigator->get_global_position()->lat;
+		mission_item->lon = _navigator->get_global_position()->lon;
+		mission_item->altitude = _navigator->get_global_position()->alt;
+	}
+
+	mission_item->altitude_is_relative = false;
+}
+
+void
+Mission::set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next)
+{
+	mission_item->nav_cmd = NAV_CMD_WAYPOINT;
+	copy_positon_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current));
+	mission_item->altitude_is_relative = false;
+	mission_item->autocontinue = true;
+	mission_item->time_inside = 0;
+	mission_item->yaw = get_bearing_to_next_waypoint(
+		_navigator->get_global_position()->lat,
+		_navigator->get_global_position()->lon,
+		mission_item_next->lat,
+		mission_item_next->lon);
+	mission_item->force_heading = true;
+}
+
 float
 Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
 {

src/modules/navigator/mission.h

@@ -119,11 +119,6 @@ private:
 	 */
 	void set_mission_items();
 
-	/**
-	 * Copies position from setpoint if valid, otherwise copies current position
-	 */
-	void copy_positon_if_valid(struct mission_item_s &mission_item, struct position_setpoint_s &setpoint);
-
 	/**
 	 * Returns true if we need to do a takeoff at the current state
 	 */
@@ -134,6 +129,16 @@ private:
 	 */
 	bool do_need_move_to_land();
 
+	/**
+	 * Copies position from setpoint if valid, otherwise copies current position
+	 */
+	void copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint);
+
+	/**
+	 * Create mission item to align towards next waypoint
+	 */
+	void set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next);
+
 	/**
 	 * Calculate takeoff height for mission item considering ground clearance
 	 */
@@ -226,7 +231,6 @@ 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,
@@ -252,7 +256,9 @@ private:
 		WORK_ITEM_TYPE_TAKEOFF,		/**< takeoff before moving to waypoint */
 		WORK_ITEM_TYPE_MOVE_TO_LAND,	/**< move to land waypoint before descent */
 		WORK_ITEM_TYPE_ALIGN,		/**< align for next waypoint */
-		WORK_ITEM_TYPE_CMD_BEFORE_MOVE	/**<  */
+		WORK_ITEM_TYPE_CMD_BEFORE_MOVE,	/**<  */
+		WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,	/**<  */
+		WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND	/**<  */
 	} _work_item_type;	/**< current type of work to do (sub mission item) */
 
 };