[RTL] Add VTOL landing approach for safe points the same way as defined for the home position.

Perform this VTOL landing approach if optional safe point loiter position are defined

src/modules/navigator/navigation.h

@@ -138,6 +138,19 @@ enum NAV_FRAME {
 #pragma GCC diagnostic error "-Wpadded"
 #endif // GCC >= 5 || Clang
 
+/**
+ * @brief Landing position.
+ * Defines the global 3D position and landing yaw.
+ *
+ */
+struct LandingPosition_s {
+	double lat;	/**< latitude in WGS84 [rad].*/
+	double lon;	/**< longitude in WGS84 [rad].*/
+	float alt;	/**< altitude in MSL [m].*/
+	float yaw;	/**< final yaw when landed [rad].*/
+};
+
+
 /**
  * Mission Item structure
  *

src/modules/navigator/rtl.cpp

@@ -210,7 +210,7 @@ void RTL::setRtlTypeAndDestination()
 	} else {
 		// check the closest allowed destination.
 		DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
-		RtlDirect::RtlPosition rtl_position;
+		LandingPosition_s rtl_position;
 		float rtl_alt;
 		findRtlDestination(destination_type, rtl_position, rtl_alt);
 
@@ -221,16 +221,13 @@ void RTL::setRtlTypeAndDestination()
 			break;
 
 		case DestinationType::DESTINATION_TYPE_SAFE_POINT:
-			_rtl_direct.setRtlAlt(rtl_alt);
-			_rtl_direct.setRtlPosition(rtl_position);
-			_rtl_type = RtlType::RTL_DIRECT;
-			break;
-
+			// Fall through
 		case DestinationType::DESTINATION_TYPE_HOME: {
 				// check if we can apply vtol land.
+				_rtl_vtol_land.setLandPosition(rtl_position);
 				bool vtol_land{_vehicle_status_sub.get().is_vtol &&
 					       _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
-					       _rtl_vtol_land.hasVtolHomeLandApproach()};
+					       _rtl_vtol_land.hasVtolLandApproach()};
 
 				if (vtol_land) {
 					_rtl_type = RtlType::RTL_DIRECT_SAFE_VTOL;
@@ -253,7 +250,7 @@ void RTL::setRtlTypeAndDestination()
 	}
 }
 
-void RTL::findRtlDestination(DestinationType &destination_type, RtlDirect::RtlPosition &rtl_position, float &rtl_alt)
+void RTL::findRtlDestination(DestinationType &destination_type, LandingPosition_s &rtl_position, float &rtl_alt)
 {
 	// set destination to home per default, then check if other valid landing spot is closer
 	rtl_position.alt = _home_pos_sub.get().alt;
@@ -332,7 +329,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, RtlDirect::RtlPo
 	}
 }
 
-void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position)
+void RTL::setLandPosAsDestination(LandingPosition_s &rtl_position)
 {
 	rtl_position.alt = _land_pos.alt;
 	rtl_position.lat = _land_pos.lat;
@@ -340,7 +337,7 @@ void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position)
 	rtl_position.yaw = _home_pos_sub.get().yaw;
 }
 
-void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position,
+void RTL::setSafepointAsDestination(LandingPosition_s &rtl_position,
 				    const mission_item_s &mission_safe_point)
 {
 	// There is a safe point closer than home/mission landing
@@ -368,7 +365,7 @@ void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position,
 	}
 }
 
-float RTL::calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position,
+float RTL::calculate_return_alt_from_cone_half_angle(const LandingPosition_s &rtl_position,
 		float cone_half_angle_deg)
 {
 	// horizontal distance to destination

src/modules/navigator/rtl.h

@@ -45,6 +45,7 @@
 #include <px4_platform_common/module_params.h>
 
 #include "navigator_mode.h"
+#include "navigation.h"
 #include "lib/mission/planned_mission_interface.h"
 #include "rtl_direct.h"
 #include "rtl_mission_fast.h"
@@ -97,20 +98,20 @@ private:
 	 * @brief Find RTL destination.
 	 *
 	 */
-	void findRtlDestination(DestinationType &destination_type, RtlDirect::RtlPosition &rtl_position, float &rtl_alt);
+	void findRtlDestination(DestinationType &destination_type, LandingPosition_s &rtl_position, float &rtl_alt);
 
 	/**
 	 * @brief Set the position of the land start marker in the planned mission as destination.
 	 *
 	 */
-	void setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position);
+	void setLandPosAsDestination(LandingPosition_s &rtl_position);
 
 	/**
 	 * @brief Set the safepoint as destination.
 	 *
 	 * @param mission_safe_point is the mission safe point/rally point to set as destination.
 	 */
-	void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_item_s &mission_safe_point);
+	void setSafepointAsDestination(LandingPosition_s &rtl_position, const mission_item_s &mission_safe_point);
 
 	/**
 	 * @brief
@@ -118,7 +119,7 @@ private:
 	 * @param cone_half_angle_deg
 	 * @return float
 	 */
-	float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, float cone_half_angle_deg);
+	float calculate_return_alt_from_cone_half_angle(const LandingPosition_s &rtl_position, float cone_half_angle_deg);
 
 	hrt_abstime _destination_check_time{0};
 

src/modules/navigator/rtl_direct.h

@@ -45,6 +45,7 @@
 
 #include "mission_block.h"
 #include "lib/mission/planned_mission_interface.h"
+#include "navigation.h"
 
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/home_position.h>
@@ -61,18 +62,6 @@ class Navigator;
 class RtlDirect : public MissionBlock, public ModuleParams
 {
 public:
-	/**
-	 * @brief Return to launch position.
-	 * Defines the position and landing yaw for the return to launch destination.
-	 *
-	 */
-	struct RtlPosition {
-		double lat;	/**< latitude in WGS84 [rad].*/
-		double lon;	/**< longitude in WGS84 [rad].*/
-		float alt;	/**< altitude in MSL [m].*/
-		float yaw;	/**< final yaw when landed [rad].*/
-	};
-
 	RtlDirect(Navigator *navigator);
 
 	~RtlDirect() = default;
@@ -101,7 +90,7 @@ public:
 
 	void setRtlAlt(float alt) {_rtl_alt = alt;};
 
-	void setRtlPosition(RtlPosition position) {_destination = position;};
+	void setRtlPosition(LandingPosition_s position) {_destination = position;};
 
 private:
 	/**
@@ -189,7 +178,7 @@ private:
 	/** Current state in the state machine.*/
 	RTLState _rtl_state{RTL_STATE_NONE};
 
-	RtlPosition _destination{}; ///< the RTL position to fly to
+	LandingPosition_s _destination{}; ///< the RTL position to fly to
 
 	float _rtl_alt{0.0f};	///< AMSL altitude at which the vehicle should return to the home position
 

src/modules/navigator/vtol_land.cpp

@@ -55,11 +55,9 @@ void
 VtolLand::on_activation()
 {
 	_global_pos_sub.update();
-	_home_pos_sub.update();
 	_wind_sub.update();
 
-	_land_pos_lat_lon = matrix::Vector2<double>(_home_pos_sub.get().lat, _home_pos_sub.get().lon);
-	readVtolHomeLandApproachesFromStorage();
+	readVtolLandApproachesFromStorage();
 	set_loiter_position();
 	_land_state = vtol_land_state::MOVE_TO_LOITER;
 
@@ -68,7 +66,6 @@ VtolLand::on_activation()
 void VtolLand::on_inactive()
 {
 	_global_pos_sub.update();
-	_home_pos_sub.update();
 	_wind_sub.update();
 }
 
@@ -76,13 +73,12 @@ void
 VtolLand::on_active()
 {
 	_global_pos_sub.update();
-	_home_pos_sub.update();
 	_wind_sub.update();
 
 	if (is_mission_item_reached_or_completed()) {
 		switch	(_land_state) {
 		case vtol_land_state::MOVE_TO_LOITER: {
-				_mission_item.altitude = _home_pos_sub.get().alt + _land_approach.height_m;
+				_mission_item.altitude = _destination.alt + _land_approach.height_m;
 				_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
 				_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
 
@@ -179,9 +175,9 @@ VtolLand::on_active()
 	}
 }
 
-bool VtolLand::hasVtolHomeLandApproach()
+bool VtolLand::hasVtolLandApproach()
 {
-	readVtolHomeLandApproachesFromStorage();
+	readVtolLandApproachesFromStorage();
 	return _vtol_home_land_approaches.isAnyApproachValid();
 }
 
@@ -196,14 +192,14 @@ VtolLand::set_loiter_position()
 		_loiter_pos_lat_lon(1) = _land_approach.lon;
 
 	} else {
-		_loiter_pos_lat_lon(0) = _home_pos_sub.get().lat;
-		_loiter_pos_lat_lon(1) = _home_pos_sub.get().lon;
+		_loiter_pos_lat_lon(0) = _destination.lat;
+		_loiter_pos_lat_lon(1) = _destination.lon;
 	}
 
 
 	_mission_item.lat  = _loiter_pos_lat_lon(0);
 	_mission_item.lon = _loiter_pos_lat_lon(1);
-	_mission_item.altitude = math::max(_home_pos_sub.get().alt + _param_return_alt_rel_m.get(),
+	_mission_item.altitude = math::max(_destination.alt + _param_return_alt_rel_m.get(),
 					   _global_pos_sub.get().alt);
 	_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
 	_mission_item.force_heading = true;
@@ -240,8 +236,8 @@ loiter_point_s VtolLand::chooseBestLandingApproach()
 	for (int i = 0; i < _vtol_home_land_approaches.num_approaches_max; i++) {
 
 		if (_vtol_home_land_approaches.approaches[i].isValid()) {
-			const float wind_angle = wrap_pi(get_bearing_to_next_waypoint(_home_pos_sub.get().lat,
-							 _home_pos_sub.get().lon, _vtol_home_land_approaches.approaches[i].lat,
+			const float wind_angle = wrap_pi(get_bearing_to_next_waypoint(_destination.lat,
+							 _destination.lon, _vtol_home_land_approaches.approaches[i].lat,
 							 _vtol_home_land_approaches.approaches[i].lon) - wind_direction);
 
 			if (fabsf(wind_angle) < wind_angle_prev) {
@@ -261,22 +257,18 @@ loiter_point_s VtolLand::chooseBestLandingApproach()
 	}
 }
 
-void VtolLand::readVtolHomeLandApproachesFromStorage()
+void VtolLand::readVtolLandApproachesFromStorage()
 {
 
 	// go through all mission items in the rally point storage. If we find a mission item of type NAV_CMD_RALLY_POINT
-	// which is within MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES  of our current home position then treat ALL following mission items of type NAV_CMD_LOITER_TO_ALT which come
-	// BEFORE the next mission item of type NAV_CMD_RALLY_POINT as land approaches for the home position
+	// which is within MAX_DIST_FROM_LAND_FOR_APPROACHES of our current pestination position then treat ALL following mission items of type NAV_CMD_LOITER_TO_ALT which come
+	// BEFORE the next mission item of type NAV_CMD_RALLY_POINT as land approaches for the destination
 	_vtol_home_land_approaches.resetAllApproaches();
 
-	if (!(_home_pos_sub.get().valid_alt && _home_pos_sub.get().valid_hpos)) {
-		return;
-	}
-
 	mission_stats_entry_s stats;
 	int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
 	int num_mission_items = 0;
-	bool foundHomeLandApproaches = false;
+	bool foundLandApproaches = false;
 	uint8_t sector_counter = 0;
 
 	if (ret == sizeof(mission_stats_entry_s)) {
@@ -294,22 +286,22 @@ void VtolLand::readVtolHomeLandApproachesFromStorage()
 
 		if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) {
 
-			if (foundHomeLandApproaches) {
+			if (foundLandApproaches) {
 				break;
 			}
 
-			const float dist_to_home = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, _home_pos_sub.get().lat,
-						   _home_pos_sub.get().lon);
+			const float dist_to_land = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, _destination.lat,
+						   _destination.lon);
 
-			if (!mission_item.is_mission_rally_point && dist_to_home < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
-				foundHomeLandApproaches = true;
+			if (!mission_item.is_mission_rally_point && dist_to_land < MAX_DIST_FROM_LAND_FOR_APPROACHES) {
+				foundLandApproaches = true;
 				_vtol_home_land_approaches.land_location_lat_lon = matrix::Vector2d(mission_item.lat, mission_item.lon);
 			}
 
 			sector_counter = 0;
 		}
 
-		if (foundHomeLandApproaches && mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
+		if (foundLandApproaches && mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
 			_vtol_home_land_approaches.approaches[sector_counter].lat = mission_item.lat;
 			_vtol_home_land_approaches.approaches[sector_counter].lon = mission_item.lon;
 			_vtol_home_land_approaches.approaches[sector_counter].height_m = mission_item.altitude;

src/modules/navigator/vtol_land.h

@@ -40,6 +40,7 @@
 #pragma once
 
 #include "navigator_mode.h"
+#include "navigation.h"
 #include "mission_block.h"
 #include "safe_point_land.hpp"
 
@@ -63,7 +64,9 @@ public:
 	void on_active() override;
 	void on_inactive() override;
 
-	bool hasVtolHomeLandApproach();
+	bool hasVtolLandApproach();
+
+	void setLandPosition(LandingPosition_s position){_destination = position;};
 
 	rtl_time_estimate_s calc_rtl_time_estimate();
 private:
@@ -88,19 +91,20 @@ private:
 		(ParamFloat<px4::params::RTL_LOITER_RAD>) _param_rtl_loiter_rad
 	)
 
+	LandingPosition_s _destination;
+
 	void set_loiter_position();
 
 	loiter_point_s chooseBestLandingApproach();
 
 	land_approaches_s _vtol_home_land_approaches{};
 
-	void readVtolHomeLandApproachesFromStorage();
+	void readVtolLandApproachesFromStorage();
 
-	static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES =
+	static constexpr float MAX_DIST_FROM_LAND_FOR_APPROACHES =
 		10.0; // [m] We don't consider home land approaches valid if the distance from the current home to the land location is greater than this distance
 
 	uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)};	/**< global position subscription */
-	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
 	uORB::SubscriptionData<wind_s>		_wind_sub{ORB_ID(wind)};
 
 };