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.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 *has_next_position_item);
+	bool prepare_mission_items(mission_item_s *mission_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,19 +574,97 @@ Navigator::run()
 			navigation_mode_new = &_rcLoss;
 			break;
 
-		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
-			_pos_sp_triplet_published_invalid_once = false;
+		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
-			if (mission_landing_required() && on_mission_landing()) {
-				navigation_mode_new = &_mission;
+				const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
-			} else {
-				navigation_mode_new = &_rtl;
+				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 {
+						navigation_mode_new = &_rtl;
+					}
+
+					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;
 			}
 
-			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
-RTL::mission_landing_required()
+int
+RTL::rtl_type() const
 {
-	// returns true if navigator should use planned mission landing
-	return (_param_rtl_land_type.get() == 1);
+	return _param_rtl_type.get();
 }
 
 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();
 
+	// 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);
+
+	// compute the return altitude
+	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()) {
+		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: {
 
-			// 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
-			float climb_alt = home.alt + _param_return_alt.get();
-
-			// 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();
-			}
-
 			_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;
-			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+			set_vtol_transition_item(&_mission_item, 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 1 Planned Landing (Mission)
+ * @value 0 Return home via direct path
+ * @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);