Navigator Mission RTL (#8749)

* Add return to land to mission

This method uses the planned mission for rtl. If a landing sequence
is present it will continue the mission and land. If not it will
fly back the mission and loiter/land at the home position.

cmake/configs/nuttx_px4fmu-v2_default.cmake

@@ -8,6 +8,7 @@ set(config_module_list
 	#
 	#drivers/barometer
 	drivers/differential_pressure
+	#drivers/distance_sensor
 	#drivers/magnetometer
 	#drivers/telemetry
 
@@ -47,7 +48,7 @@ set(config_module_list
 
 	# distance sensors
 	drivers/distance_sensor/ll40ls
-	drivers/distance_sensor/mb12xx
+	#drivers/distance_sensor/mb12xx
 	drivers/distance_sensor/sf0x
 	drivers/distance_sensor/sf1xx
 	drivers/distance_sensor/srf02

msg/mission_result.msg

@@ -1,3 +1,7 @@
+uint8 MISSION_EXECUTION_MODE_NORMAL = 0	# Execute the mission according to the planned items
+uint8 MISSION_EXECUTION_MODE_REVERSE = 1	# Execute the mission in reverse order, ignoring commands and converting all waypoints to normal ones
+uint8 MISSION_EXECUTION_MODE_FAST_FORWARD = 2	# Execute the mission as fast as possible, for example converting loiter waypoints to normal ones
+
 uint32 instance_count		# Instance count of this mission. Increments monotonically whenever the mission is modified
 
 int32 seq_reached		# Sequence of the mission item which has been reached, default -1
@@ -15,3 +19,5 @@ bool flight_termination		# true if the navigator demands a flight termination fr
 bool item_do_jump_changed	# true if the number of do jumps remaining has changed
 uint16 item_changed_index	# indicate which item has changed
 uint16 item_do_jump_remaining	# set to the number of do jumps remaining for that item
+
+uint8 execution_mode	# indicates the mode in which the mission is executed

src/modules/navigator/mission.cpp

@@ -48,6 +48,7 @@
 #include "mission.h"
 #include "navigator.h"
 
+#include <string.h>
 #include <drivers/drv_hrt.h>
 #include <dataman/dataman.h>
 #include <systemlib/mavlink_log.h>
@@ -84,6 +85,10 @@ Mission::on_inactive()
 
 		if (offboard_updated) {
 			update_offboard_mission();
+
+			if (_mission_type == MISSION_TYPE_NONE && _offboard_mission.count > 0) {
+				_mission_type = MISSION_TYPE_OFFBOARD;
+			}
 		}
 
 		/* reset the current offboard mission if needed */
@@ -147,6 +152,14 @@ Mission::on_inactivation()
 void
 Mission::on_activation()
 {
+	if (_mission_waypoints_changed) {
+		_current_offboard_mission_index = index_closest_mission_item();
+		_mission_waypoints_changed = false;
+	}
+
+	// we already reset the mission items
+	_execution_mode_changed = false;
+
 	set_mission_items();
 
 	// unpause triggering if it was paused
@@ -179,8 +192,16 @@ Mission::on_active()
 		offboard_updated = true;
 	}
 
+	_mission_changed = false;
+
 	/* reset mission items if needed */
-	if (offboard_updated) {
+	if (offboard_updated || _mission_waypoints_changed || _execution_mode_changed) {
+		if (_mission_waypoints_changed) {
+			_current_offboard_mission_index = index_closest_mission_item();
+			_mission_waypoints_changed = false;
+		}
+
+		_execution_mode_changed = false;
 		set_mission_items();
 	}
 
@@ -251,6 +272,10 @@ Mission::set_current_offboard_mission_index(uint16_t index)
 
 		_current_offboard_mission_index = index;
 
+		// a mission offboard index is set manually which has the higher priority than the closest mission item
+		// as it is set by the user
+		_mission_waypoints_changed = false;
+
 		// update mission items if already in active mission
 		if (_navigator->is_planned_mission()) {
 			// prevent following "previous - current" line
@@ -266,6 +291,76 @@ Mission::set_current_offboard_mission_index(uint16_t index)
 	return false;
 }
 
+void
+Mission::set_closest_item_as_current()
+{
+	_current_offboard_mission_index = index_closest_mission_item();
+}
+
+void
+Mission::set_execution_mode(const uint8_t mode)
+{
+	if (_mission_execution_mode != mode) {
+		_execution_mode_changed = true;
+		_navigator->get_mission_result()->execution_mode = mode;
+
+
+		switch (_mission_execution_mode) {
+		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
+		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD:
+			if (mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
+				// command a transition if in vtol mc mode
+				if (_navigator->get_vstatus()->is_rotary_wing &&
+				    _navigator->get_vstatus()->is_vtol &&
+				    !_navigator->get_land_detected()->landed) {
+
+					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
+
+					position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+					pos_sp_triplet->previous = pos_sp_triplet->current;
+					generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
+					_navigator->set_position_setpoint_triplet_updated();
+					issue_command(_mission_item);
+				}
+
+				if (_mission_type == MISSION_TYPE_NONE && _offboard_mission.count > 0) {
+					_mission_type = MISSION_TYPE_OFFBOARD;
+				}
+
+				if (_current_offboard_mission_index > _offboard_mission.count - 1) {
+					_current_offboard_mission_index = _offboard_mission.count - 1;
+
+				} else if (_current_offboard_mission_index > 0) {
+					--_current_offboard_mission_index;
+				}
+
+				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+			}
+
+			break;
+
+		case mission_result_s::MISSION_EXECUTION_MODE_REVERSE:
+			if ((mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL) ||
+			    (mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD)) {
+				// handle switch from reverse to forward mission
+				if (_current_offboard_mission_index < 0) {
+					_current_offboard_mission_index = 0;
+
+				} else if (_current_offboard_mission_index < _offboard_mission.count - 1) {
+					++_current_offboard_mission_index;
+				}
+
+				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+			}
+
+			break;
+
+		}
+
+		_mission_execution_mode = mode;
+	}
+}
+
 bool
 Mission::find_offboard_land_start()
 {
@@ -287,7 +382,9 @@ Mission::find_offboard_land_start()
 			break;
 		}
 
-		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
+		if ((missionitem.nav_cmd == NAV_CMD_DO_LAND_START) ||
+		    ((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) ||
+		    (missionitem.nav_cmd == NAV_CMD_LAND)) {
 			_land_start_available = true;
 			_land_start_index = i;
 			return true;
@@ -330,11 +427,14 @@ Mission::landing()
 void
 Mission::update_offboard_mission()
 {
+
 	bool failed = true;
 
 	/* reset triplets */
 	_navigator->reset_triplets();
 
+	struct mission_s old_offboard_mission = _offboard_mission;
+
 	if (orb_copy(ORB_ID(mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
 		/* determine current index */
 		if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
@@ -367,6 +467,15 @@ Mission::update_offboard_mission()
 
 			/* reset work item if new mission has been accepted */
 			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+			_mission_changed = true;
+		}
+
+		/* check if the mission waypoints changed while the vehicle is in air
+		 * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
+		if (((_offboard_mission.count != old_offboard_mission.count) ||
+		     (_offboard_mission.dataman_id != old_offboard_mission.dataman_id)) &&
+		    !_navigator->get_land_detected()->landed) {
+			_mission_waypoints_changed = true;
 		}
 
 	} else {
@@ -398,8 +507,43 @@ Mission::advance_mission()
 
 	switch (_mission_type) {
 	case MISSION_TYPE_OFFBOARD:
+		switch (_mission_execution_mode) {
+		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
+		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
 				_current_offboard_mission_index++;
 				break;
+			}
+
+		case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: {
+				// find next position item in reverse order
+				dm_item_t dm_current = (dm_item_t)(_offboard_mission.dataman_id);
+
+				for (int32_t i = _current_offboard_mission_index - 1; i >= 0; i--) {
+					struct mission_item_s missionitem = {};
+					const ssize_t len = sizeof(missionitem);
+
+					if (dm_read(dm_current, i, &missionitem, len) != len) {
+						/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+						PX4_ERR("dataman read failure");
+						break;
+					}
+
+					if (item_contains_position(missionitem)) {
+						_current_offboard_mission_index = i;
+						return;
+					}
+				}
+
+				// finished flying back the mission
+				_current_offboard_mission_index = -1;
+				break;
+			}
+
+		default:
+			_current_offboard_mission_index++;
+		}
+
+		break;
 
 	case MISSION_TYPE_NONE:
 	default:
@@ -407,17 +551,6 @@ Mission::advance_mission()
 	}
 }
 
-float
-Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
-{
-	if (mission_item.altitude_is_relative) {
-		return mission_item.altitude + _navigator->get_home_position()->alt;
-
-	} else {
-		return mission_item.altitude;
-	}
-}
-
 void
 Mission::set_mission_items()
 {
@@ -436,7 +569,9 @@ Mission::set_mission_items()
 	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing mission.");
+			mavlink_log_info(_navigator->get_mavlink_log_pub(),
+					 _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Executing Reverse Mission" :
+					 "Executing Mission");
 			user_feedback_done = true;
 		}
 
@@ -447,11 +582,15 @@ Mission::set_mission_items()
 		if (_mission_type != MISSION_TYPE_NONE) {
 
 			if (_navigator->get_land_detected()->landed) {
-				mavlink_log_info(_navigator->get_mavlink_log_pub(), "Mission finished, landed.");
+				mavlink_log_info(_navigator->get_mavlink_log_pub(),
+						 _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, landed" :
+						 "Mission finished, landed.");
 
 			} else {
 				/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
-				mavlink_log_info(_navigator->get_mavlink_log_pub(), "Mission finished, loitering.");
+				mavlink_log_info(_navigator->get_mavlink_log_pub(),
+						 _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, loitering" :
+						 "Mission finished, loitering.");
 
 				/* use last setpoint for loiter */
 				_navigator->set_can_loiter_at_sp(true);
@@ -502,9 +641,14 @@ Mission::set_mission_items()
 
 	/*********************************** handle mission item *********************************************/
 
-	/* handle position mission items */
+	/* handle mission items depending on the mode */
-	if (item_contains_position(_mission_item)) {
 
+	const position_setpoint_s current_setpoint_copy = _navigator->get_position_setpoint_triplet()->current;
+
+	if (item_contains_position(_mission_item)) {
+		switch (_mission_execution_mode) {
+		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
+		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
 				/* force vtol land */
 				if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
 					_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
@@ -512,11 +656,6 @@ Mission::set_mission_items()
 
 				position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-		/* we have a new position item so set previous position setpoint to current */
-		if (_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) {
-			pos_sp_triplet->previous = pos_sp_triplet->current;
-		}
-
 				/* do takeoff before going to setpoint if needed and not already in takeoff */
 				/* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */
 				if (do_need_vertical_takeoff() &&
@@ -603,9 +742,7 @@ Mission::set_mission_items()
 						new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
 					}
 
-			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
+					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
-			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
-			_mission_item.yaw = _navigator->get_global_position()->yaw;
 
 					/* set position setpoint to target during the transition */
 					// TODO: if has_next_position_item and use_next set next, or if use_heading set generated
@@ -656,9 +793,8 @@ Mission::set_mission_items()
 				    && !_navigator->get_vstatus()->is_rotary_wing
 				    && !_navigator->get_land_detected()->landed) {
 
-			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
+					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
-			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+
-			_mission_item.autocontinue = true;
 					new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
 				}
 
@@ -737,9 +873,41 @@ Mission::set_mission_items()
 					_mission_item.yaw = NAN;
 				}
 
+
+				// for fast forward convert certain types to simple waypoint
+				// XXX: add other types which should be ignored in fast forward
+				if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD &&
+				    ((_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) ||
+				     (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT))) {
+					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+					_mission_item.autocontinue = true;
+					_mission_item.time_inside = 0.0f;
+				}
+
+				break;
+			}
+
+		case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: {
+				if (item_contains_position(_mission_item)) {
+					// convert mission item to a simple waypoint
+					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+					_mission_item.autocontinue = true;
+					_mission_item.time_inside = 0.0f;
+
+				} else {
+					mavlink_log_critical(_navigator->get_mavlink_log_pub(), "MissionReverse: Got a non-position mission item, ignoring it");
+				}
+
+				break;
+			}
+		}
+
 	} else {
-		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		/* handle non-position mission items such as commands */
+		switch (_mission_execution_mode) {
+		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
+		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
+				position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
 				/* turn towards next waypoint before MC to FW transition */
 				if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
@@ -788,9 +956,25 @@ 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_position_if_valid(&_mission_item, &pos_sp_triplet->current);
 					_mission_item.autocontinue = true;
-			_mission_item.time_inside = 0;
+					_mission_item.time_inside = 0.0f;
+				}
+
+				// ignore certain commands in mission fast forward
+				if ((_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD) &&
+				    (_mission_item.nav_cmd == NAV_CMD_DELAY)) {
+					_mission_item.autocontinue = true;
+					_mission_item.time_inside = 0.0f;
+				}
+
+				break;
+			}
+
+		case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: {
+				// nothing to do, all commands are ignored
+				break;
+			}
 		}
 	}
 
@@ -804,6 +988,12 @@ Mission::set_mission_items()
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	}
 
+	/* only set the previous position item if the current one really changed */
+	if ((_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) &&
+	    !position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
+		pos_sp_triplet->previous = current_setpoint_copy;
+	}
+
 	/* issue command if ready (will do nothing for position mission items) */
 	issue_command(_mission_item);
 
@@ -828,7 +1018,7 @@ Mission::set_mission_items()
 		/* try to process next mission item */
 		if (has_next_position_item) {
 			/* got next mission item, update setpoint triplet */
-			mission_apply_limitation(_mission_item);
+			mission_apply_limitation(mission_item_next_position);
 			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
 
 		} else {
@@ -920,7 +1110,7 @@ Mission::do_need_move_to_takeoff()
 }
 
 void
-Mission::copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint)
+Mission::copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint)
 {
 	if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
 		mission_item->lat = setpoint->lat;
@@ -940,7 +1130,7 @@ 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));
+	copy_position_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.0f;
@@ -1220,12 +1410,17 @@ Mission::do_abort_landing()
 }
 
 bool
-Mission::prepare_mission_items(mission_item_s *mission_item, mission_item_s *next_position_mission_item,
+Mission::prepare_mission_items(mission_item_s *mission_item,
-			       bool *has_next_position_item)
+			       mission_item_s *next_position_mission_item, bool *has_next_position_item)
 {
+	*has_next_position_item = false;
 	bool first_res = false;
 	int offset = 1;
 
+	if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
+		offset = -1;
+	}
+
 	if (read_mission_item(0, mission_item)) {
 
 		first_res = true;
@@ -1238,7 +1433,13 @@ Mission::prepare_mission_items(mission_item_s *mission_item, mission_item_s *nex
 				break;
 			}
 
+			if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
+				offset--;
+
+			} else {
 				offset++;
+
+			}
 		}
 	}
 
@@ -1249,27 +1450,25 @@ bool
 Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
 {
 	/* select offboard mission */
-	struct mission_s *mission = &_offboard_mission;
+	const int current_index = _current_offboard_mission_index;
-	int current_index = _current_offboard_mission_index;
 	int index_to_read = current_index + offset;
 
 	int *mission_index_ptr = (offset == 0) ? &_current_offboard_mission_index : &index_to_read;
 	const dm_item_t dm_item = (dm_item_t)_offboard_mission.dataman_id;
 
 	/* do not work on empty missions */
-	if (mission->count == 0) {
+	if (_offboard_mission.count == 0) {
 		return false;
 	}
 
 	/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
 	 * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
 	for (int i = 0; i < 10; i++) {
-
+		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)_offboard_mission.count) {
-		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
 			/* mission item index out of bounds - if they are equal, we just reached the end */
-			if (*mission_index_ptr != (int)mission->count) {
+			if ((*mission_index_ptr != (int)_offboard_mission.count) && (*mission_index_ptr != -1)) {
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission item index out of bound, index: %d, max: %d.",
-						     *mission_index_ptr, mission->count);
+						     *mission_index_ptr, _offboard_mission.count);
 			}
 
 			return false;
@@ -1289,9 +1488,10 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
 
 		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
 		if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) {
+			const bool execute_jumps = _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL;
 
-			/* do DO_JUMP as many times as requested */
+			/* do DO_JUMP as many times as requested if not in reverse mode */
-			if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) {
+			if ((mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) && execute_jumps) {
 
 				/* only raise the repeat count if this is for the current mission item
 				 * but not for the read ahead mission item */
@@ -1313,13 +1513,18 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
 				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
 
 			} else {
-				if (offset == 0) {
+				if (offset == 0 && execute_jumps) {
 					mavlink_log_info(_navigator->get_mavlink_log_pub(), "DO JUMP repetitions completed.");
 				}
 
 				/* no more DO_JUMPS, therefore just try to continue with next mission item */
+				if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
+					(*mission_index_ptr)--;
+
+				} else {
 					(*mission_index_ptr)++;
 				}
+			}
 
 		} else {
 			/* if it's not a DO_JUMP, then we were successful */
@@ -1516,3 +1721,96 @@ Mission::generate_waypoint_from_heading(struct position_setpoint_s *setpoint, fl
 	setpoint->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 	setpoint->yaw = yaw;
 }
+
+int32_t
+Mission::index_closest_mission_item() const
+{
+	int32_t min_dist_index(0);
+	float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX);
+
+	dm_item_t dm_current = (dm_item_t)(_offboard_mission.dataman_id);
+
+	for (size_t i = 0; i < _offboard_mission.count; i++) {
+		struct mission_item_s missionitem = {};
+		const ssize_t len = sizeof(missionitem);
+
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			PX4_ERR("dataman read failure");
+			break;
+		}
+
+		if (item_contains_position(missionitem)) {
+			// do not consider land waypoints for a fw
+			if (!((missionitem.nav_cmd == NAV_CMD_LAND) &&
+			      (!_navigator->get_vstatus()->is_rotary_wing) &&
+			      (!_navigator->get_vstatus()->is_vtol))) {
+				float dist = get_distance_to_point_global_wgs84(missionitem.lat, missionitem.lon,
+						get_absolute_altitude_for_item(missionitem),
+						_navigator->get_global_position()->lat,
+						_navigator->get_global_position()->lon,
+						_navigator->get_global_position()->alt,
+						&dist_xy, &dist_z);
+
+				if (dist < min_dist) {
+					min_dist = dist;
+					min_dist_index = i;
+				}
+			}
+		}
+	}
+
+	// for mission reverse also consider the home position
+	if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
+		float dist = get_distance_to_point_global_wgs84(
+				     _navigator->get_home_position()->lat,
+				     _navigator->get_home_position()->lon,
+				     _navigator->get_home_position()->alt,
+				     _navigator->get_global_position()->lat,
+				     _navigator->get_global_position()->lon,
+				     _navigator->get_global_position()->alt,
+				     &dist_xy, &dist_z);
+
+		if (dist < min_dist) {
+			min_dist = dist;
+			min_dist_index = -1;
+		}
+	}
+
+	return min_dist_index;
+}
+
+bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const
+{
+	return ((p1->valid == p2->valid) &&
+		(p1->type == p2->type) &&
+		(fabsf(p1->x - p2->x) < FLT_EPSILON) &&
+		(fabsf(p1->y - p2->y) < FLT_EPSILON) &&
+		(fabsf(p1->z - p2->z) < FLT_EPSILON) &&
+		(p1->position_valid == p2->position_valid) &&
+		(fabsf(p1->vx - p2->vx) < FLT_EPSILON) &&
+		(fabsf(p1->vy - p2->vy) < FLT_EPSILON) &&
+		(fabsf(p1->vz - p2->vz) < FLT_EPSILON) &&
+		(p1->velocity_valid == p2->velocity_valid) &&
+		(p1->velocity_frame == p2->velocity_frame) &&
+		(p1->alt_valid == p2->alt_valid) &&
+		(fabs(p1->lat - p2->lat) < DBL_EPSILON) &&
+		(fabs(p1->lon - p2->lon) < DBL_EPSILON) &&
+		(fabsf(p1->alt - p2->alt) < FLT_EPSILON) &&
+		((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) &&
+		(p1->yaw_valid == p2->yaw_valid) &&
+		(fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) &&
+		(p1->yawspeed_valid == p2->yawspeed_valid) &&
+		(fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) &&
+		(p1->loiter_direction == p2->loiter_direction) &&
+		(fabsf(p1->pitch_min - p2->pitch_min) < FLT_EPSILON) &&
+		(fabsf(p1->a_x - p2->a_x) < FLT_EPSILON) &&
+		(fabsf(p1->a_y - p2->a_y) < FLT_EPSILON) &&
+		(fabsf(p1->a_z - p2->a_z) < FLT_EPSILON) &&
+		(p1->acceleration_valid == p2->acceleration_valid) &&
+		(p1->acceleration_is_force == p2->acceleration_is_force) &&
+		(fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) &&
+		(fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) &&
+		(fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON));
+
+}

src/modules/navigator/mission.h

@@ -94,7 +94,19 @@ public:
 	bool landing();
 
 	uint16_t get_land_start_index() const { return _land_start_index; }
+	bool get_land_start_available() const { return _land_start_available; }
+	bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; }
+	bool get_mission_changed() const { return _mission_changed ; }
+	bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; }
 
+	void set_closest_item_as_current();
+
+	/**
+	 * Set a new mission mode and handle the switching between the different modes
+	 *
+	 * For a list of the different modes refer to mission_result.msg
+	 */
+	void set_execution_mode(const uint8_t mode);
 private:
 
 	/**
@@ -130,7 +142,7 @@ private:
 	/**
 	 * 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);
+	void copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint);
 
 	/**
 	 * Create mission item to align towards next waypoint
@@ -162,16 +174,14 @@ private:
 	 */
 	void do_abort_landing();
 
-	float get_absolute_altitude_for_item(struct mission_item_s &mission_item);
-
 	/**
 	 * Read the current and the next mission item. The next mission item read is the
 	 * next mission item that contains a position.
 	 *
 	 * @return true if current mission item available
 	 */
-	bool prepare_mission_items(mission_item_s *mission_item, mission_item_s *next_position_mission_item,
+	bool prepare_mission_items(mission_item_s *mission_item,
-				   bool *has_next_position_item);
+				   mission_item_s *next_position_mission_item, bool *has_next_position_item);
 
 	/**
 	 * Read current (offset == 0) or a specific (offset > 0) mission item
@@ -179,7 +189,7 @@ private:
 	 *
 	 * @return true if successful
 	 */
-	bool read_mission_item(int offset, mission_item_s *mission_item);
+	bool read_mission_item(int offset, struct mission_item_s *mission_item);
 
 	/**
 	 * Save current offboard mission state to dataman
@@ -206,7 +216,6 @@ private:
 	 */
 	void check_mission_valid(bool force);
 
-
 	/**
 	 * Reset offboard mission
 	 */
@@ -227,6 +236,13 @@ private:
 	 */
 	bool find_offboard_land_start();
 
+	/**
+	 * Return the index of the closest offboard mission item to the current global position.
+	 */
+	int32_t index_closest_mission_item() const;
+
+	bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MIS_DIST_1WP>) _param_dist_1wp,
 		(ParamFloat<px4::params::MIS_DIST_WPS>) _param_dist_between_wps,
@@ -253,6 +269,8 @@ private:
 	bool _inited{false};
 	bool _home_inited{false};
 	bool _need_mission_reset{false};
+	bool _mission_waypoints_changed{false};
+	bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */
 
 	float _min_current_sp_distance_xy{FLT_MAX}; /**< minimum distance which was achieved to the current waypoint  */
 
@@ -269,4 +287,7 @@ private:
 		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
 		WORK_ITEM_TYPE_PRECISION_LAND
 	} _work_item_type{WORK_ITEM_TYPE_DEFAULT};	/**< current type of work to do (sub mission item) */
+
+	uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL};	/**< the current mode of how the mission is executed,look at mission_result.msg for the definition */
+	bool _execution_mode_changed{false};
 };

src/modules/navigator/mission_block.cpp

@@ -677,6 +677,15 @@ MissionBlock::set_idle_item(struct mission_item_s *item)
 	item->origin = ORIGIN_ONBOARD;
 }
 
+void
+MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode)
+{
+	item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
+	item->params[0] = (float) new_mode;
+	item->yaw = _navigator->get_global_position()->yaw;
+	item->autocontinue = true;
+}
+
 void
 MissionBlock::mission_apply_limitation(mission_item_s &item)
 {
@@ -705,3 +714,14 @@ MissionBlock::mission_apply_limitation(mission_item_s &item)
 	 * Add other limitations here
 	 */
 }
+
+float
+MissionBlock::get_absolute_altitude_for_item(struct mission_item_s &mission_item) const
+{
+	if (mission_item.altitude_is_relative) {
+		return mission_item.altitude + _navigator->get_home_position()->alt;
+
+	} else {
+		return mission_item.altitude;
+	}
+}

src/modules/navigator/mission_block.h

@@ -107,6 +107,11 @@ protected:
 	 */
 	void set_idle_item(struct mission_item_s *item);
 
+	/**
+	 * Set vtol transition item
+	 */
+	void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode);
+
 	/**
 	 * General function used to adjust the mission item based on vehicle specific limitations
 	 */
@@ -116,6 +121,8 @@ protected:
 
 	float get_time_inside(const struct mission_item_s &item);
 
+	float get_absolute_altitude_for_item(struct mission_item_s &mission_item) const;
+
 	mission_item_s _mission_item{};
 
 	bool _waypoint_position_reached{false};

src/modules/navigator/navigator.h

@@ -250,9 +250,12 @@ public:
 	bool		is_planned_mission() const { return _navigation_mode == &_mission; }
 	bool		on_mission_landing() { return _mission.landing(); }
 	bool		start_mission_landing() { return _mission.land_start(); }
+	bool		mission_start_land_available() { return _mission.get_land_start_available(); }
 
 	// RTL
-	bool		mission_landing_required() { return _rtl.mission_landing_required(); }
+	bool		mission_landing_required() { return _rtl.rtl_type() == RTL::RTL_LAND; }
+	int			rtl_type() { return _rtl.rtl_type(); }
+	bool		in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }
 
 	bool		abort_landing();
 
@@ -268,7 +271,6 @@ public:
 	bool		force_vtol();
 
 private:
-
 	int		_fw_pos_ctrl_status_sub{-1};	/**< notification of vehicle capabilities updates */
 	int		_global_pos_sub{-1};		/**< global position subscription */
 	int		_gps_pos_sub{-1};		/**< gps position subscription */
@@ -298,7 +300,7 @@ private:
 	vehicle_land_detected_s				_land_detected{};	/**< vehicle land_detected */
 	vehicle_local_position_s			_local_pos{};		/**< local vehicle position */
 	vehicle_status_s				_vstatus{};		/**< vehicle status */
-
+	uint8_t					_previous_nav_state{}; /**< nav_state of the previous iteration*/
 	// Publications
 	geofence_result_s				_geofence_result{};
 	position_setpoint_triplet_s			_pos_sp_triplet{};	/**< triplet of position setpoints */

src/modules/navigator/navigator_main.cpp

@@ -111,7 +111,6 @@ Navigator::Navigator() :
 	_navigation_mode_array[8] = &_land;
 	_navigation_mode_array[9] = &_precland;
 	_navigation_mode_array[10] = &_follow_target;
-
 }
 
 void
@@ -559,7 +558,10 @@ Navigator::run()
 		switch (_vstatus.nav_state) {
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 			_pos_sp_triplet_published_invalid_once = false;
+
+			_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_NORMAL);
 			navigation_mode_new = &_mission;
+
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
@@ -572,11 +574,20 @@ Navigator::run()
 			navigation_mode_new = &_rcLoss;
 			break;
 
-		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: {
 				_pos_sp_triplet_published_invalid_once = false;
 
-			// if RTL is set to use a mission landing and mission has a planned landing, then use MISSION
+				const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
-			if (mission_landing_required() && on_mission_landing()) {
+
+				switch (rtl_type()) {
+				case RTL::RTL_LAND:
+					if (rtl_activated) {
+						mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL LAND activated");
+					}
+
+					// if RTL is set to use a mission landing and mission has a planned landing, then use MISSION to fly there directly
+					if (on_mission_landing() && !get_land_detected()->landed) {
+						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
 						navigation_mode_new = &_mission;
 
 					} else {
@@ -585,6 +596,75 @@ Navigator::run()
 
 					break;
 
+				case RTL::RTL_MISSION:
+					if (_mission.get_land_start_available() && !get_land_detected()->landed) {
+						// the mission contains a landing spot
+						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
+
+						if (_navigation_mode != &_mission) {
+							if (_navigation_mode == nullptr) {
+								// switching from an manual mode, go to landing if not already landing
+								if (!on_mission_landing()) {
+									start_mission_landing();
+								}
+
+							} else {
+								// switching from an auto mode, continue the mission from the closest item
+								_mission.set_closest_item_as_current();
+							}
+						}
+
+						if (rtl_activated) {
+							mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission");
+						}
+
+						navigation_mode_new = &_mission;
+
+					} else {
+						// fly the mission in reverse if switching from a non-manual mode
+						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE);
+
+						if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) &&
+						    (! _mission.get_mission_finished()) &&
+						    (!get_land_detected()->landed)) {
+							// determine the closest mission item if switching from a non-mission mode, and we are either not already
+							// mission mode or the mission waypoints changed.
+							// The seconds condition is required so that when no mission was uploaded and one is available the closest
+							// mission item is determined and also that if the user changes the active mission index while rtl is active
+							// always that waypoint is tracked first.
+							if ((_navigation_mode != &_mission) && (rtl_activated || _mission.get_mission_waypoints_changed())) {
+								_mission.set_closest_item_as_current();
+							}
+
+							if (rtl_activated) {
+								mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse");
+							}
+
+							navigation_mode_new = &_mission;
+
+						} else {
+							if (rtl_activated) {
+								mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home");
+							}
+
+							navigation_mode_new = &_rtl;
+						}
+					}
+
+					break;
+
+				default:
+					if (rtl_activated) {
+						mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL HOME activated");
+					}
+
+					navigation_mode_new = &_rtl;
+					break;
+				}
+
+				break;
+			}
+
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
 			_pos_sp_triplet_published_invalid_once = false;
 			navigation_mode_new = &_takeoff;
@@ -640,6 +720,9 @@ Navigator::run()
 			break;
 		}
 
+		// update the vehicle status
+		_previous_nav_state = _vstatus.nav_state;
+
 		/* we have a new navigation mode: reset triplet */
 		if (_navigation_mode != navigation_mode_new) {
 			// We don't reset the triplet if we just did an auto-takeoff and are now

src/modules/navigator/rtl.cpp

@@ -63,11 +63,10 @@ RTL::on_inactive()
 	_rtl_state = RTL_STATE_NONE;
 }
 
-bool
+int
-RTL::mission_landing_required()
+RTL::rtl_type() const
 {
-	// returns true if navigator should use planned mission landing
+	return _param_rtl_type.get();
-	return (_param_rtl_land_type.get() == 1);
 }
 
 void
@@ -77,9 +76,8 @@ RTL::on_activation()
 		// for safety reasons don't go into RTL if landed
 		_rtl_state = RTL_STATE_LANDED;
 
-	} else if (mission_landing_required() && _navigator->on_mission_landing()) {
+	} else if ((rtl_type() == RTL_LAND) && _navigator->on_mission_landing()) {
 		// RTL straight to RETURN state, but mission will takeover for landing
-		_rtl_state = RTL_STATE_RETURN;
 
 	} else if ((_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get())
 		   || _rtl_alt_min) {
@@ -117,7 +115,7 @@ RTL::set_rtl_item()
 	// RTL_TYPE: mission landing
 	// landing using planned mission landing, fly to DO_LAND_START instead of returning HOME
 	// do nothing, let navigator takeover with mission landing
-	if (mission_landing_required()) {
+	if (rtl_type() == RTL_LAND) {
 		if (_rtl_state > RTL_STATE_CLIMB) {
 			if (_navigator->start_mission_landing()) {
 				mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing");
@@ -137,24 +135,27 @@ RTL::set_rtl_item()
 
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	switch (_rtl_state) {
-	case RTL_STATE_CLIMB: {
-
 	// check if we are pretty close to home already
 	const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
 
-			// if we are close to home we do not climb as high, otherwise we climb to return alt
+	// compute the return altitude
-			float climb_alt = home.alt + _param_return_alt.get();
+	float return_alt = max(home.alt + _param_return_alt.get(), gpos.alt);
 
 	// we are close to home, limit climb to min
 	if (home_dist < _param_rtl_min_dist.get()) {
-				climb_alt = home.alt + _param_descend_alt.get();
+		return_alt = home.alt + _param_descend_alt.get();
 	}
 
+	// compute the loiter altitude
+	const float loiter_altitude = min(home.alt + _param_descend_alt.get(), gpos.alt);
+
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB: {
+
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.lat = gpos.lat;
 			_mission_item.lon = gpos.lon;
-			_mission_item.altitude = climb_alt;
+			_mission_item.altitude = return_alt;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.yaw = NAN;
 			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
@@ -163,7 +164,7 @@ RTL::set_rtl_item()
 			_mission_item.origin = ORIGIN_ONBOARD;
 
 			mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
-						     (int)ceilf(climb_alt), (int)ceilf(climb_alt - _navigator->get_home_position()->alt));
+						     (int)ceilf(return_alt), (int)ceilf(return_alt - _navigator->get_home_position()->alt));
 			break;
 		}
 
@@ -173,13 +174,11 @@ RTL::set_rtl_item()
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.lat = home.lat;
 			_mission_item.lon = home.lon;
-			_mission_item.altitude = gpos.alt;
+			_mission_item.altitude = return_alt;
 			_mission_item.altitude_is_relative = false;
 
 			// use home yaw if close to home
 			/* check if we are pretty close to home already */
-			const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
-
 			if (home_dist < _param_rtl_min_dist.get()) {
 				_mission_item.yaw = home.yaw;
 
@@ -200,8 +199,7 @@ RTL::set_rtl_item()
 		}
 
 	case RTL_STATE_TRANSITION_TO_MC: {
-			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
+			set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
-			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
 			break;
 		}
 
@@ -209,7 +207,7 @@ RTL::set_rtl_item()
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.lat = home.lat;
 			_mission_item.lon = home.lon;
-			_mission_item.altitude = min(home.alt + _param_descend_alt.get(), gpos.alt);
+			_mission_item.altitude = loiter_altitude;
 			_mission_item.altitude_is_relative = false;
 
 			// except for vtol which might be still off here and should point towards this location
@@ -241,7 +239,7 @@ RTL::set_rtl_item()
 			// don't change altitude
 			_mission_item.lat = home.lat;
 			_mission_item.lon = home.lon;
-			_mission_item.altitude = gpos.alt;
+			_mission_item.altitude = loiter_altitude;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.yaw = home.yaw;
 			_mission_item.loiter_radius = _navigator->get_loiter_radius();

src/modules/navigator/rtl.h

@@ -50,7 +50,14 @@ class Navigator;
 class RTL : public MissionBlock, public ModuleParams
 {
 public:
+	enum RTLType {
+		RTL_HOME = 0,
+		RTL_LAND,
+		RTL_MISSION,
+	};
+
 	RTL(Navigator *navigator);
+
 	~RTL() = default;
 
 	void on_inactive() override;
@@ -59,7 +66,7 @@ public:
 
 	void set_return_alt_min(bool min);
 
-	bool mission_landing_required();
+	int rtl_type() const;
 
 private:
 	/**
@@ -90,6 +97,6 @@ private:
 		(ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_descend_alt,
 		(ParamFloat<px4::params::RTL_LAND_DELAY>) _param_land_delay,
 		(ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist,
-		(ParamInt<px4::params::RTL_LAND_TYPE>) _param_rtl_land_type
+		(ParamInt<px4::params::RTL_TYPE>) _param_rtl_type
 	)
 };

src/modules/navigator/rtl_params.c

@@ -105,12 +105,14 @@ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
 PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 5.0f);
 
 /**
- * RTL land location
+ * Return type
  *
- * Land at the home location or planned mission landing
+ * Fly straight to the home location or planned mission landing and land there or
+ * use the planned mission to get to those points.
  *
- * @value 0 Home Position
+ * @value 0 Return home via direct path
- * @value 1 Planned Landing (Mission)
+ * @value 1 Return to a planned mission landing, if available, via direct path, else return to home via direct path
+ * @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path
  * @group Return To Land
  */
-PARAM_DEFINE_INT32(RTL_LAND_TYPE, 0);
+PARAM_DEFINE_INT32(RTL_TYPE, 0);