From d0286481c5c0ab4d9ddaa3fc3958e9c02bab26ff Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Tue, 23 Jun 2026 12:24:04 +0200
Subject: [PATCH] feat(fw-mission): do not return to line when large position
 reset occurs

---
 .../fw_mode_manager/FixedWingModeManager.cpp     | 16 ++++++++++++++++
 .../fw_mode_manager/FixedWingModeManager.hpp     |  3 +++
 .../fw_mode_manager/fw_mode_manager_params.yaml  | 14 ++++++++++++++
 3 files changed, 33 insertions(+)

diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp
index 21feb2a2303..14bc3b3871b 100644
--- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp
+++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp
@@ -2110,6 +2110,16 @@ FixedWingModeManager::Run()
 							     _local_pos.ref_timestamp);
 		}
 
+		const float max_reset_dist = _param_fw_wp_rst_dist.get();
+
+		if (_control_mode.flag_control_auto_enabled
+		    && (_local_pos.xy_reset_counter != _xy_reset_counter)
+		    && (max_reset_dist > FLT_EPSILON)
+		    && (Vector2f(_local_pos.delta_xy).longerThan(max_reset_dist))) {
+			// Large position reset, directly go to destination to avoid strange path corrections
+			_go_direct_to_destination = true;
+		}
+
 		if (_control_mode.flag_control_offboard_enabled) {
 			trajectory_setpoint_s trajectory_setpoint;
 
@@ -2183,6 +2193,8 @@ FixedWingModeManager::Run()
 
 				// reset the altitude foh (first order hold) logic
 				_min_current_sp_distance_xy = FLT_MAX;
+
+				_go_direct_to_destination = false;
 			}
 		}
 
@@ -2633,6 +2645,10 @@ DirectionalGuidanceOutput FixedWingModeManager::navigateWaypoints(const Vector2f
 		return navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel);
 	}
 
+	if (_go_direct_to_destination) {
+		return navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel);
+	}
+
 	// follow the line segment between the start and end waypoints
 	return navigateLine(start_waypoint, end_waypoint, vehicle_pos, ground_vel, wind_vel);
 }
diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp
index 23759e7adeb..4862daab031 100644
--- a/src/modules/fw_mode_manager/FixedWingModeManager.hpp
+++ b/src/modules/fw_mode_manager/FixedWingModeManager.hpp
@@ -385,6 +385,7 @@ private:
 	uint64_t _time_last_xy_reset{0};
 
 	// LATERAL-DIRECTIONAL GUIDANCE
+	bool _go_direct_to_destination{false};
 
 	// CLosest point on path to track
 	matrix::Vector2f _closest_point_on_path;
@@ -846,6 +847,8 @@ private:
 		(ParamFloat<px4::params::NPFG_SW_DST_MLT>) _param_npfg_switch_distance_multiplier,
 		(ParamFloat<px4::params::NPFG_PERIOD_SF>) _param_npfg_period_safety_factor,
 
+		(ParamFloat<px4::params::FW_WP_RST_DIST>) _param_fw_wp_rst_dist,
+
 		(ParamFloat<px4::params::FW_LND_AIRSPD>) _param_fw_lnd_airspd,
 		(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
 		(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,
diff --git a/src/modules/fw_mode_manager/fw_mode_manager_params.yaml b/src/modules/fw_mode_manager/fw_mode_manager_params.yaml
index f3695c92b6f..be5e39714ed 100644
--- a/src/modules/fw_mode_manager/fw_mode_manager_params.yaml
+++ b/src/modules/fw_mode_manager/fw_mode_manager_params.yaml
@@ -517,3 +517,17 @@ parameters:
       max: 10.0
       decimal: 1
       increment: 0.1
+    FW_WP_RST_DIST:
+      description:
+        short: Max position reset distance
+        long: |-
+          If a horizontal position reset larger than this occurs while flying between two
+          waypoints, fly directly to the current waypoint instead of rejoining the line.
+          -1 to disable.
+      type: float
+      default: -1.0
+      unit: m
+      min: -1.0
+      max: 10000.0
+      decimal: 0
+      increment: 1