Add mission check land when no approach

msg/CMakeLists.txt

@@ -173,6 +173,7 @@ set(msg_files
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
 	Rpm.msg
+	RtlStatus.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg
 	SensorAccel.msg

msg/RtlStatus.msg

@@ -0,0 +1,15 @@
+uint64 timestamp                      # time since system start (microseconds)
+
+uint32 safe_points_id 		      # unique ID of active set of safe_point_items
+bool is_evaluation_pending 	      # flag if the RTL point needs reevaluation (e.g. new safe points available, but need loading).
+
+bool has_vtol_approach 		      # flag if approaches are defined for current RTL_TYPE parameter setting
+
+uint8 rtl_type	      		      # Type of RTL chosen
+uint8 safe_point_index 		      # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode
+
+uint8 RTL_STATUS_TYPE_NONE=0       # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points
+uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position
+uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing
+uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 	# RTL type is following the mission from closest point to mission landing
+uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position

src/modules/logger/logged_topics.cpp

@@ -102,6 +102,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("px4io_status");
 	add_topic("radio_status");
 	add_topic("rtl_time_estimate", 1000);
+	add_optional_topic("rtl_status", 5000);
 	add_optional_topic("sensor_airflow", 100);
 	add_topic("sensor_combined");
 	add_optional_topic("sensor_correction");

src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp

@@ -125,6 +125,12 @@ void FeasibilityChecker::updateData()
 		_current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
 	}
 
+	if (_rtl_status_sub.updated()) {
+		rtl_status_s rtl_status = {};
+		_rtl_status_sub.copy(&rtl_status);
+		_has_vtol_approach = rtl_status.has_vtol_approach;
+	}
+
 	param_t handle = param_find("FW_LND_ANG");
 
 	if (handle != PARAM_INVALID) {
@@ -577,17 +583,22 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
 		break;
 
 	case 4:
-		result = _has_takeoff == _landing_valid;
+		result = hasMissionBothOrNeitherTakeoffAndLanding();
+
+		break;
 
-		if (!result && (_has_takeoff)) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Landing item or remove Takeoff.\t");
-			events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: Add Landing item or remove Takeoff");
+	case 5:
+		if (!_is_landed && !_has_vtol_approach) {
+			result = _landing_valid;
 
-		} else if (!result && (_landing_valid)) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Takeoff item or remove Landing.\t");
-			events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: Add Takeoff item or remove Landing");
+			if (!result) {
+				mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Landing waypoint/pattern required.");
+				events::send(events::ID("feasibility_mis_in_air_landing_req"), {events::Log::Error, events::LogInternal::Info},
+					     "Mission rejected: Landing waypoint/pattern required");
+			}
+
+		} else {
+			result = hasMissionBothOrNeitherTakeoffAndLanding();
 		}
 
 		break;
@@ -600,6 +611,23 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
 	return result;
 }
 
+bool FeasibilityChecker::hasMissionBothOrNeitherTakeoffAndLanding()
+{
+	bool result{_has_takeoff == _landing_valid};
+
+	if (!result && (_has_takeoff)) {
+		mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Landing item or remove Takeoff.\t");
+		events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: Add Landing item or remove Takeoff");
+
+	} else if (!result && (_landing_valid)) {
+		mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Takeoff item or remove Landing.\t");
+		events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: Add Takeoff item or remove Landing");
+	}
+
+	return result;
+}
 
 bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item)
 {

src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp

@@ -36,6 +36,7 @@
 #include "../navigation.h"
 #include <mathlib/mathlib.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/rtl_status.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
@@ -97,6 +98,7 @@ class FeasibilityChecker : public ModuleParams
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription _rtl_status_sub{ORB_ID(rtl_status)};
 
 	// parameters
 	float _param_fw_lnd_ang{0.f};
@@ -106,6 +108,7 @@ class FeasibilityChecker : public ModuleParams
 
 	bool _is_landed{false};
 	float _home_alt_msl{NAN};
+	bool _has_vtol_approach{false};
 	matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 	matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 	VehicleType _vehicle_type{VehicleType::RotaryWing};
@@ -247,4 +250,8 @@ class FeasibilityChecker : public ModuleParams
 	 * @return False if the check failed.
 	*/
 	void doMulticopterChecks(mission_item_s &mission_item, const int current_index);
+
+	// Helper functions
+
+	bool hasMissionBothOrNeitherTakeoffAndLanding();
 };

src/modules/navigator/mission_params.c

@@ -68,6 +68,7 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
  * @value 2 Require a landing
  * @value 3 Require a takeoff and a landing
  * @value 4 Require both a takeoff and a landing, or neither
+ * @value 5 Same as previous, but require a landing if in air and no valid VTOL landing approach is present
  * @group Mission
  */
 PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);

src/modules/navigator/navigator_main.cpp

@@ -208,7 +208,7 @@ void Navigator::run()
 
 			if (mission.safe_points_id != safe_points_id) {
 				safe_points_id = mission.safe_points_id;
-				_rtl.updateSafePoints();
+				_rtl.updateSafePoints(safe_points_id);
 			}
 		}
 

src/modules/navigator/rtl.cpp

@@ -294,12 +294,14 @@ void RTL::setRtlTypeAndDestination()
 
 	init_rtl_mission_type();
 
+	uint8_t safe_point_index{0U};
+
 	if (_param_rtl_type.get() != 2) {
 		// check the closest allowed destination.
 		DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
 		PositionYawSetpoint rtl_position;
 		float rtl_alt;
-		findRtlDestination(destination_type, rtl_position, rtl_alt);
+		findRtlDestination(destination_type, rtl_position, rtl_alt, safe_point_index);
 
 		switch (destination_type) {
 		case DestinationType::DESTINATION_TYPE_MISSION_LAND:
@@ -331,9 +333,29 @@ void RTL::setRtlTypeAndDestination()
 			break;
 		}
 	}
+
+	// Publish rtl status
+	_rtl_status_pub.get().timestamp = hrt_absolute_time();
+	_rtl_status_pub.get().safe_points_id = _safe_points_id;
+	_rtl_status_pub.get().is_evaluation_pending = _dataman_state != DatamanState::UpdateRequestWait;
+	_rtl_status_pub.get().has_vtol_approach = false;
+
+	if ((_param_rtl_type.get() == 0) || (_param_rtl_type.get() == 3)) {
+		_rtl_status_pub.get().has_vtol_approach = _home_has_land_approach || _one_rally_point_has_land_approach;
+
+	} else if (_param_rtl_type.get() == 1) {
+		_rtl_status_pub.get().has_vtol_approach = _one_rally_point_has_land_approach;
+	}
+
+	_rtl_status_pub.get().rtl_type = static_cast<uint8_t>(_rtl_type);
+	_rtl_status_pub.get().safe_point_index = safe_point_index;
+
+	_rtl_status_pub.update();
+
 }
 
-void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt)
+void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
+			     uint8_t &safe_point_index)
 {
 	// set destination to home per default, then check if other valid landing spot is closer
 	rtl_position.alt = _home_pos_sub.get().alt;
@@ -352,8 +374,10 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 	float home_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)};
 	float min_dist;
 
+	_home_has_land_approach = hasVtolLandApproach(rtl_position);
+
 	if (((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) || (vtol_in_fw_mode && (_param_rtl_approach_force.get() == 1)
-			&& !hasVtolLandApproach(rtl_position))) {
+			&& !_home_has_land_approach)) {
 		// Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we forces approach landing for vtol in fw and it is not defined for home.
 		min_dist = FLT_MAX;
 
@@ -394,6 +418,8 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 
 	if (_safe_points_updated) {
 
+		_one_rally_point_has_land_approach = false;
+
 		for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) {
 			mission_item_s mission_safe_point;
 
@@ -416,11 +442,16 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 				PositionYawSetpoint safepoint_position;
 				setSafepointAsDestination(safepoint_position, mission_safe_point);
 
+				bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)};
+
+				_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
+
 				if (((dist + MIN_DIST_THRESHOLD) < min_dist) && (!vtol_in_fw_mode || (_param_rtl_approach_force.get() == 0)
-						|| hasVtolLandApproach(safepoint_position))) {
+						|| current_safe_point_has_approaches)) {
 					min_dist = dist;
 					rtl_position = safepoint_position;
 					destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
+					safe_point_index = current_seq;
 				}
 			}
 		}

src/modules/navigator/rtl.h

@@ -58,6 +58,7 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/rtl_status.h>
 #include <uORB/topics/rtl_time_estimate.h>
 
 class Navigator;
@@ -86,7 +87,7 @@ class RTL : public NavigatorMode, public ModuleParams
 
 	void set_return_alt_min(bool min) { _enforce_rtl_alt = min; }
 
-	void updateSafePoints() { _initiate_safe_points_updated = true; }
+	void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; }
 
 private:
 	enum class DestinationType {
@@ -109,7 +110,8 @@ class RTL : public NavigatorMode, public ModuleParams
 	 * @brief Find RTL destination.
 	 *
 	 */
-	void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt);
+	void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
+				uint8_t &safe_point_index);
 
 	/**
 	 * @brief Set the position of the land start marker in the planned mission as destination.
@@ -188,6 +190,9 @@ class RTL : public NavigatorMode, public ModuleParams
 
 	RtlType _rtl_type{RtlType::RTL_DIRECT};
 
+	bool _home_has_land_approach;			///< Flag if the home position has a land approach defined
+	bool _one_rally_point_has_land_approach;	///< Flag if a rally point has a land approach defined
+
 	DatamanState _dataman_state{DatamanState::UpdateRequestWait};
 	DatamanState _error_state{DatamanState::UpdateRequestWait};
 	uint32_t _opaque_id{0}; ///< dataman safepoint id: if it does not match, safe points data was updated
@@ -197,6 +202,7 @@ class RTL : public NavigatorMode, public ModuleParams
 	bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed
 	mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
 	uint32_t _mission_id = 0u;
+	uint32_t _safe_points_id = 0u;
 
 	mission_stats_entry_s _stats;
 
@@ -222,4 +228,5 @@ class RTL : public NavigatorMode, public ModuleParams
 	uORB::SubscriptionData<wind_s>		_wind_sub{ORB_ID(wind)};
 
 	uORB::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
+	uORB::PublicationData<rtl_status_s> _rtl_status_pub{ORB_ID(rtl_status)};
 };