chore(navigator): move return to point of last link from RTL to Hold

Give the operator the optiont to configure a "Hold at position where
the data link was still coming through" by setting NAV_DLL_ACT to Hold
and the new param NAV_LTR_LAST_DL to 1.

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

src/modules/navigator/loiter.cpp

@@ -73,6 +73,22 @@ Loiter::on_active()
 	    && hrt_elapsed_time(&_navigator->get_reposition_triplet()->current.timestamp) < 500_ms) {
 		reposition();
 	}
+
+	if (_param_nav_ltr_last_dl.get() && _navigator->get_vstatus()->failsafe && _navigator->get_vstatus()->gcs_connection_lost) {
+		if (!_loiter_at_last_link_position_executed) {
+			set_loiter_position(); // if we already were in hold (e.g. GoTo), we need to reset the position setpoint
+			_loiter_at_last_link_position_executed = true;
+		}
+
+	} else {
+		_loiter_at_last_link_position_executed = false;
+	}
+}
+
+void
+Loiter::on_inactive()
+{
+	_loiter_at_last_link_position_executed = false;
 }
 
 void
@@ -87,7 +103,6 @@ Loiter::set_loiter_position()
 		_navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
 		_navigator->set_position_setpoint_triplet_updated();
 		return;
-
 	}
 
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -104,10 +119,13 @@ Loiter::set_loiter_position()
 			const float d_current = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
 						_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
 			on_loiter = d_current <= (_navigator->get_acceptance_radius() + pos_sp_triplet->current.loiter_radius);
-
 		}
 
-		if (on_loiter) {
+		if (_navigator->get_vstatus()->failsafe && _navigator->get_vstatus()->gcs_connection_lost && _param_nav_ltr_last_dl.get()) {
+
+			setLoiterFromLastLink(&_mission_item);
+
+		} else if (on_loiter) {
 			setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
 
 		} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
@@ -116,7 +134,6 @@ Loiter::set_loiter_position()
 		} else {
 			setLoiterItemFromCurrentPosition(&_mission_item);
 		}
-
 	}
 
 	// convert mission item to current setpoint

src/modules/navigator/loiter.h

@@ -53,6 +53,7 @@ public:
 
 	void on_activation() override;
 	void on_active() override;
+	void on_inactive() override;
 
 private:
 	/**
@@ -66,4 +67,9 @@ private:
 	 */
 	void set_loiter_position();
 
+	bool _loiter_at_last_link_position_executed{false};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::NAV_LTR_LAST_DL>) _param_nav_ltr_last_dl
+	)
 };

src/modules/navigator/mission_block.cpp

@@ -760,6 +760,19 @@ MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item)
 	item->origin = ORIGIN_ONBOARD;
 }
 
+void
+MissionBlock::setLoiterFromLastLink(struct mission_item_s *item)
+{
+	setLoiterItemCommonFields(item);
+
+	const PositionYawSetpoint &last_heartbeat_pos = _navigator->get_last_pos_with_gcs_heartbeat();
+
+	item->lat = PX4_ISFINITE(last_heartbeat_pos.lat) ? last_heartbeat_pos.lat : _navigator->get_global_position()->lat;
+	item->lon = PX4_ISFINITE(last_heartbeat_pos.lon) ? last_heartbeat_pos.lon : _navigator->get_global_position()->lon;
+	item->altitude = PX4_ISFINITE(last_heartbeat_pos.alt) ? last_heartbeat_pos.alt : _navigator->get_global_position()->alt;
+	item->yaw = PX4_ISFINITE(last_heartbeat_pos.yaw) ? last_heartbeat_pos.yaw : NAN;
+}
+
 void
 MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude)
 {

src/modules/navigator/mission_block.h

@@ -165,6 +165,7 @@ protected:
 
 	void setLoiterItemFromCurrentPosition(struct mission_item_s *item);
 	void setLoiterItemFromCurrentPositionWithBraking(struct mission_item_s *item);
+	void setLoiterFromLastLink(struct mission_item_s *item);
 
 	void setLoiterItemCommonFields(struct mission_item_s *item);
 

src/modules/navigator/navigator.h

@@ -57,6 +57,9 @@
 
 #include "GeofenceBreachAvoidance/geofence_breach_avoidance.h"
 
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/telemetry_status.h>
+
 #if CONFIG_NAVIGATOR_ADSB
 #include <lib/adsb/AdsbConflict.h>
 #endif // CONFIG_NAVIGATOR_ADSB
@@ -174,6 +177,8 @@ public:
 
 	PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */
 
+	const PositionYawSetpoint &get_last_pos_with_gcs_heartbeat() const { return _last_pos_with_gcs_heartbeat; }
+
 	const vehicle_roi_s &get_vroi() { return _vroi; }
 
 	void reset_vroi() { _vroi = {}; }
@@ -402,6 +407,9 @@ private:
 
 	bool _is_capturing_images{false}; // keep track if we need to stop capturing images
 
+	uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
+	PositionYawSetpoint _last_pos_with_gcs_heartbeat{(double)NAN, (double)NAN, NAN, NAN};
+
 
 	// timer to trigger a delayed set gimbal neutral command
 	hrt_abstime _gimbal_neutral_activation_time{UINT64_MAX};

src/modules/navigator/navigator_main.cpp

@@ -225,6 +225,19 @@ void Navigator::run()
 			_global_pos_sub.copy(&_global_pos);
 		}
 
+		/* update last known position with GCS heartbeat */
+		for (auto &telemetry_sub : _telemetry_status_subs) {
+			telemetry_status_s telemetry;
+
+			if (telemetry_sub.update(&telemetry) && telemetry.heartbeat_type_gcs) {
+				_last_pos_with_gcs_heartbeat.lat = _global_pos.lat;
+				_last_pos_with_gcs_heartbeat.lon = _global_pos.lon;
+				_last_pos_with_gcs_heartbeat.alt = _global_pos.alt;
+				_last_pos_with_gcs_heartbeat.yaw = _local_pos.heading;
+				break;
+			}
+		}
+
 		/* check for parameter updates */
 		if (_parameter_update_sub.updated()) {
 			// clear update

src/modules/navigator/navigator_params.yaml

@@ -140,3 +140,13 @@ parameters:
       min: -1
       decimal: 1
       increment: 1
+    NAV_LTR_LAST_DL:
+      description:
+        short: Loiter at last GCS heartbeat position on data link loss
+        long: |-
+          When the data link is lost and this setting is enabled,
+          the vehicle will loiter at the position where the last GCS
+          heartbeat was received rather than at its current position.
+          Only applies to Hold mode during failsafe actions.
+      type: boolean
+      default: 0

src/modules/navigator/rtl.cpp

@@ -536,32 +536,11 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 		destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
 
 	} else if (_param_rtl_type.get() == 5) {
-		// Safe points only but no valid safe point, fallback to last position with valid data link
+		// for RTL_TYPE=5: if no rally point is found fallback to current position
-		for (auto &telemetry_status :  _telemetry_status_subs) {
+		destination.alt = _global_pos_sub.get().alt;
-			telemetry_status_s telemetry;
+		destination.lat = _global_pos_sub.get().lat;
-
+		destination.lon = _global_pos_sub.get().lon;
-			if (telemetry_status.update(&telemetry)) {
+		destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
-
-				if (telemetry.heartbeat_type_gcs) {
-					_last_position_before_link_loss.alt = _global_pos_sub.get().alt;
-					_last_position_before_link_loss.lat = _global_pos_sub.get().lat;
-					_last_position_before_link_loss.lon = _global_pos_sub.get().lon;
-					break;
-				}
-			}
-		}
-
-		if (PX4_ISFINITE(_last_position_before_link_loss.lat) && PX4_ISFINITE(_last_position_before_link_loss.lon)) {
-			destination = _last_position_before_link_loss;
-
-		} else {
-			// If no valid data link position, fallback to current position
-			destination.alt = _global_pos_sub.get().alt;
-			destination.lat = _global_pos_sub.get().lat;
-			destination.lon = _global_pos_sub.get().lon;
-		}
-
-		destination_type = DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION;
 	}
 }
 
@@ -600,11 +579,6 @@ void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mis
 
 float RTL::computeReturnAltitude(const PositionYawSetpoint &rtl_position, DestinationType destination_type, float cone_half_angle_deg) const
 {
-	if (destination_type == DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION) {
-		// when returning to last known link position, do not modify altitude
-		return rtl_position.alt;
-	}
-
 	if (_param_rtl_cone_ang.get() > 0 && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 		// horizontal distance to destination
 		const float destination_dist =

src/modules/navigator/rtl.h

@@ -55,13 +55,11 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
-#include <uORB/SubscriptionMultiArray.hpp>
 #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>
-#include <uORB/topics/telemetry_status.h>
 
 class Navigator;
 
@@ -96,8 +94,7 @@ private:
 	enum class DestinationType {
 		DESTINATION_TYPE_HOME,
 		DESTINATION_TYPE_MISSION_LAND,
-		DESTINATION_TYPE_SAFE_POINT,
+		DESTINATION_TYPE_SAFE_POINT
-		DESTINATION_TYPE_LAST_LINK_POSITION
 	};
 
 private:
@@ -229,7 +226,6 @@ private:
 	mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
 	uint32_t _mission_id = 0u;
 	uint32_t _safe_points_id = 0u;
-	PositionYawSetpoint _last_position_before_link_loss{(double)NAN, (double)NAN, NAN, NAN};
 
 	mission_stats_entry_s _stats;
 
@@ -253,7 +249,6 @@ private:
 	uORB::SubscriptionData<mission_s> _mission_sub{ORB_ID(mission)};
 	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};
 	uORB::SubscriptionData<wind_s>		_wind_sub{ORB_ID(wind)};
-	uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
 
 	uORB::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
 	uORB::Publication<rtl_status_s> _rtl_status_pub{ORB_ID(rtl_status)};