RTL: publish a status message on currently chosen RTL point

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/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 @@ public:
 
 	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 @@ private:
 	 * @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 @@ private:
 
 	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 @@ private:
 	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 @@ private:
 	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)};
 };