Safe Landing Points: Added new RTL_TYPE: closest between home, mission landing and safe points

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

Co-authored by Martina Rivizzigno <martina@rivizzigno.it>

src/modules/navigator/mission.cpp

@@ -376,17 +376,17 @@ Mission::set_execution_mode(const uint8_t mode)
 bool
 Mission::find_mission_land_start()
 {
-	/* return true if a MAV_CMD_DO_LAND_START is found and internally save the index
+	/* return true if a MAV_CMD_DO_LAND_START, NAV_CMD_VTOL_LAND or NAV_CMD_LAND is found and internally save the index
 	 *  return false if not found
-	 *
-	 * TODO: implement full spec and find closest landing point geographically
 	 */
 
 	const dm_item_t dm_current = (dm_item_t)_mission.dataman_id;
+	struct mission_item_s missionitem = {};
+	struct mission_item_s missionitem_prev = {}; //to store mission item before currently checked on, needed to get pos of wp before NAV_CMD_DO_LAND_START
 
-	for (size_t i = 0; i < _mission.count; i++) {
-		struct mission_item_s missionitem = {};
+	for (size_t i = 1; i < _mission.count; i++) {
 		const ssize_t len = sizeof(missionitem);
+		missionitem_prev = missionitem; // store the last mission item before reading a new one
 
 		if (dm_read(dm_current, i, &missionitem, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@@ -394,11 +394,27 @@ Mission::find_mission_land_start()
 			break;
 		}
 
-		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)) {
+		// first check for DO_LAND_START marker
+		if ((missionitem.nav_cmd == NAV_CMD_DO_LAND_START) && (missionitem_prev.nav_cmd == NAV_CMD_WAYPOINT)) {
+
+			_land_start_available = true;
+			_land_start_index = i;
+			// the DO_LAND_START marker contains no position sp, so take them from the previous mission item
+			_landing_lat = missionitem_prev.lat;
+			_landing_lon = missionitem_prev.lon;
+			_landing_alt = missionitem_prev.altitude;
+			return true;
+
+			// if no DO_LAND_START marker available, also check for VTOL_LAND or normal LAND
+
+		} else if (((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;
+			_landing_lat = missionitem.lat;
+			_landing_lon = missionitem.lon;
+			_landing_alt = missionitem.altitude;
 			return true;
 		}
 	}

src/modules/navigator/mission.h

@@ -91,6 +91,9 @@ public:
 	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 ; }
+	double get_landing_lat() { return _landing_lat; }
+	double get_landing_lon() { return _landing_lon; }
+	float get_landing_alt() { return _landing_alt; }
 
 	void set_closest_item_as_current();
 
@@ -251,6 +254,9 @@ private:
 	// track location of planned mission landing
 	bool	_land_start_available{false};
 	uint16_t _land_start_index{UINT16_MAX};		/**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */
+	double _landing_lat{0.0};
+	double _landing_lon{0.0};
+	float _landing_alt{0.0f};
 
 	bool _need_takeoff{true};					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
 

src/modules/navigator/navigator.h

@@ -268,7 +268,11 @@ 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(); }
+	bool		get_mission_start_land_available() { return _mission.get_land_start_available(); }
+	int 		get_mission_landing_index() { return _mission.get_land_start_index(); }
+	double 	get_mission_landing_lat() { return _mission.get_landing_lat(); }
+	double 	get_mission_landing_lon() { return _mission.get_landing_lon(); }
+	float 	get_mission_landing_alt() { return _mission.get_landing_alt(); }
 
 	// RTL
 	bool		mission_landing_required() { return _rtl.rtl_type() == RTL::RTL_LAND; }

src/modules/navigator/navigator_main.cpp

@@ -490,9 +490,16 @@ Navigator::run()
 				const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
 				switch (rtl_type()) {
-				case RTL::RTL_LAND:
+				case RTL::RTL_LAND: // use mission landing
+				case RTL::RTL_CLOSEST:
 					if (rtl_activated) {
-						mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL LAND activated");
+						if (rtl_type() == RTL::RTL_LAND) {
+							mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL LAND activated");
+
+						} else {
+							mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Closest landing point activated");
+						}
+
 					}
 
 					// if RTL is set to use a mission landing and mission has a planned landing, then use MISSION to fly there directly
@@ -570,6 +577,7 @@ Navigator::run()
 
 					navigation_mode_new = &_rtl;
 					break;
+
 				}
 
 				break;

src/modules/navigator/rtl.cpp

@@ -57,22 +57,49 @@ RTL::on_inactive()
 {
 	// Reset RTL state.
 	_rtl_state = RTL_STATE_NONE;
-}
 
-int
-RTL::rtl_type() const
-{
-	return _param_rtl_type.get();
+	find_RTL_destination();
+
 }
 
 void
-RTL::on_activation()
+RTL::find_closest_landing_point()
 {
-
-	// find the RTL destination: go through the safe points & home position
-	const home_position_s &home_position = *_navigator->get_home_position();
+	// get home position:
+	home_position_s &home_landing_position = *_navigator->get_home_position();
+	// get global position
 	const vehicle_global_position_s &global_position = *_navigator->get_global_position();
 
+	// set destination to home per default, then check if other valid landing spot is closer
+	_destination.set(home_landing_position);
+	// get distance to home position
+	double dlat = home_landing_position.lat - global_position.lat;
+	double dlon = home_landing_position.lon - global_position.lon;
+	double min_dist_squared = dlat * dlat + dlon * dlon;
+
+	_destination.type = RTL_DESTINATION_HOME;
+
+	if (_navigator->get_mission_start_land_available()) {
+		double mission_landing_lat = _navigator->get_mission_landing_lat();
+		double mission_landing_lon = _navigator->get_mission_landing_lon();
+
+		// compare home position to landing position to decide which is closer
+		dlat = mission_landing_lat - global_position.lat;
+		dlon = mission_landing_lon - global_position.lon;
+		double dist_squared = dlat * dlat + dlon * dlon;
+
+		if (dist_squared < min_dist_squared) {
+			min_dist_squared = dist_squared;
+			_destination.lat = _navigator->get_mission_landing_lat();
+			_destination.lon = _navigator->get_mission_landing_lon();
+			_destination.alt = _navigator->get_mission_landing_alt();
+			_destination.type = RTL_DESTINATION_MISSION_LANDING;
+
+		}
+	}
+
+	// compare to safe landing positions
+	mission_safe_point_s closest_safe_point {} ;
 	mission_stats_entry_s stats;
 	int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
 	int num_safe_points = 0;
@@ -81,15 +108,9 @@ RTL::on_activation()
 		num_safe_points = stats.num_items;
 	}
 
+	// check if a safe point is closer than home or landing
 	int closest_index = 0;
-	mission_safe_point_s closest_safe_point;
-
-	// take home position into account
-	double dlat = home_position.lat - global_position.lat;
-	double dlon = home_position.lon - global_position.lon;
-	double min_dist_squared = dlat * dlat + dlon * dlon;
 
-	// find the closest point
 	for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) {
 		mission_safe_point_s mission_safe_point;
 
@@ -100,7 +121,6 @@ RTL::on_activation()
 		}
 
 		// TODO: take altitude into account for distance measurement
-
 		dlat = mission_safe_point.lat - global_position.lat;
 		dlon = mission_safe_point.lon - global_position.lon;
 		double dist_squared = dlat * dlat + dlon * dlon;
@@ -112,43 +132,111 @@ RTL::on_activation()
 		}
 	}
 
-	if (closest_index == 0) {
-		_destination.set(home_position);
-
-	} else {
+	if (closest_index > 0) {
+		_destination.type = RTL_DESTINATION_SAFE_POINT;
 
+		// There is a safe point closer than home/mission landing
 		// TODO: handle all possible mission_safe_point.frame cases
 		switch (closest_safe_point.frame) {
 		case 0: // MAV_FRAME_GLOBAL
-			_destination.safe_point_index = closest_index;
 			_destination.lat = closest_safe_point.lat;
 			_destination.lon = closest_safe_point.lon;
 			_destination.alt = closest_safe_point.alt;
-			_destination.yaw = home_position.yaw;
+			_destination.yaw = home_landing_position.yaw;
 			break;
 
 		case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT
-			_destination.safe_point_index = closest_index;
 			_destination.lat = closest_safe_point.lat;
 			_destination.lon = closest_safe_point.lon;
-			_destination.alt = closest_safe_point.alt + home_position.alt; // alt of safe point is rel to home
-			_destination.yaw = home_position.yaw;
+			_destination.alt = closest_safe_point.alt + home_landing_position.alt; // alt of safe point is rel to home
+			_destination.yaw = home_landing_position.yaw;
 			break;
 
 		default:
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME. Landing at HOME.");
-			_destination.set(home_position);
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME");
 			break;
 		}
 	}
 
+}
+
+void
+RTL::find_RTL_destination()
+{
+	// get home position:
+	home_position_s &home_landing_position = *_navigator->get_home_position();
+
+	switch (rtl_type()) {
+	case RTL_HOME:
+		// always take home position as landing destination
+		_destination.set(home_landing_position);
+		_destination.type = RTL_DESTINATION_HOME;
+		break;
+
+	case RTL_LAND:
+
+	// take mission landing as landing destination (if available), otherwise home
+	case RTL_MISSION:
+
+		// inverse mission
+		if (_navigator->get_mission_start_land_available()) {
+			_destination.lat = _navigator->get_mission_landing_lat();
+			_destination.lon = _navigator->get_mission_landing_lon();
+			_destination.alt = _navigator->get_mission_landing_alt();
+			_destination.type = RTL_DESTINATION_MISSION_LANDING;
+
+		} else {
+			_destination.set(home_landing_position);
+			_destination.type = RTL_DESTINATION_HOME;
+		}
+
+		break;
+
+	case RTL_CLOSEST:
+		// choose closest possible landing point (consider home, mission landing and safe points)
+		find_closest_landing_point();
+		break;
+
+	default:
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported RTL_TYPE. Change RTL_TYPE param.");
+		break;
+	}
+}
+
+int
+RTL::rtl_type() const
+{
+	return _param_rtl_type.get();
+}
+
+void
+RTL::on_activation()
+{
+
+	// output the correct message, depending on where the RTL destination is
+	switch (_destination.type) {
+	case RTL_DESTINATION_HOME:
+		mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position.");
+		break;
+
+	case RTL_DESTINATION_MISSION_LANDING:
+		mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing.");
+		break;
+
+	case RTL_DESTINATION_SAFE_POINT:
+		mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point.");
+		break;
+	}
+
+	const vehicle_global_position_s &global_position = *_navigator->get_global_position();
+
 	_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
 
 	if (_navigator->get_land_detected()->landed) {
 		// For safety reasons don't go into RTL if landed.
 		_rtl_state = RTL_STATE_LANDED;
 
-	} else if ((rtl_type() == RTL_LAND) && _navigator->on_mission_landing()) {
+	} else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->on_mission_landing()) {
 		// RTL straight to RETURN state, but mission will takeover for landing.
 
 	} else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) {
@@ -185,8 +273,8 @@ RTL::set_rtl_item()
 {
 	// RTL_TYPE: mission landing.
 	// Landing using planned mission landing, fly to DO_LAND_START instead of returning _destination.
-	// Do nothing, let navigator takeover with mission landing.
-	if (rtl_type() == RTL_LAND) {
+	// After reaching DO_LAND_START, do nothing, let navigator takeover with mission landing.
+	if (_destination.type == RTL_DESTINATION_MISSION_LANDING) {
 		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");

src/modules/navigator/rtl.h

@@ -57,6 +57,13 @@ public:
 		RTL_HOME = 0,
 		RTL_LAND,
 		RTL_MISSION,
+		RTL_CLOSEST,
+	};
+
+	enum RTLDestinationType {
+		RTL_DESTINATION_HOME = 0,
+		RTL_DESTINATION_MISSION_LANDING,
+		RTL_DESTINATION_SAFE_POINT,
 	};
 
 	RTL(Navigator *navigator);
@@ -67,10 +74,15 @@ public:
 	void on_activation() override;
 	void on_active() override;
 
+	void find_closest_landing_point();
+	void find_RTL_destination();
+
 	void set_return_alt_min(bool min);
 
 	int rtl_type() const;
 
+	int rtl_destination();
+
 private:
 	/**
 	 * Set the RTL item
@@ -101,7 +113,8 @@ private:
 		double lon;
 		float alt;
 		float yaw;
-		uint8_t safe_point_index; ///< 0 = home position
+		uint8_t safe_point_index; ///< 0 = home position, 1 = mission landing, >1 = safe landing points (rally points)
+		RTLDestinationType type{RTL_DESTINATION_HOME};
 
 		void set(const home_position_s &home_position)
 		{
@@ -110,10 +123,11 @@ private:
 			alt = home_position.alt;
 			yaw = home_position.yaw;
 			safe_point_index = 0;
+			type = RTL_DESTINATION_HOME;
 		}
 	};
 
-	RTLPosition _destination; ///< the RTL position to fly to (typically the home position or a safe point)
+	RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point)
 
 	float _rtl_alt{0.0f};	// AMSL altitude at which the vehicle should return to the home position
 	bool _rtl_alt_min{false};

src/modules/navigator/rtl_params.c

@@ -113,6 +113,7 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 5.0f);
  * @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
+ * @value 3 Return via direct way to whatever is closest: home, mission lading or safe point
  * @group Return Mode
  */
 PARAM_DEFINE_INT32(RTL_TYPE, 0);