From c09c63171c90ccb5b4fcef4cfbebac5d33c0eaab Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Mon, 24 Feb 2025 09:04:42 +0100
Subject: [PATCH] MC auto: fix land nudging

Revert removal of isTargetModified as this is required when the target
is changed by "RC help" (nudging) during landing.
---
 .../tasks/Auto/FlightTaskAuto.cpp                  | 14 ++++++++++++++
 .../tasks/Auto/FlightTaskAuto.hpp                  |  1 +
 2 files changed, 15 insertions(+)

diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
index 419f25827a..556ac6c72e 100644
--- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
@@ -159,6 +159,11 @@ bool FlightTaskAuto::update()
 	_checkEmergencyBraking();
 	Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp};
 
+	if (isTargetModified()) {
+		// In case the target has been modified, we take this as the next waypoints
+		waypoints[2] = _position_setpoint;
+	}
+
 	const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == int32_t(yaw_mode::towards_waypoint_yaw_first)
 					       && !_yaw_sp_aligned;
 	const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active;
@@ -759,6 +764,15 @@ bool FlightTaskAuto::_generateHeadingAlongTraj()
 	return res;
 }
 
+bool FlightTaskAuto::isTargetModified() const
+{
+	const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON);
+	const bool z_valid = PX4_ISFINITE(_position_setpoint(2));
+	const bool z_modified =  z_valid && std::fabs((_target - _position_setpoint)(2)) > FLT_EPSILON;
+
+	return xy_modified || z_modified;
+}
+
 void FlightTaskAuto::_updateTrajConstraints()
 {
 	// update params of the position smoothing
diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
index 5b8d3e9b5b..6c49dd8726 100644
--- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
@@ -108,6 +108,7 @@ protected:
 
 	void _checkEmergencyBraking();
 	bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
+	bool isTargetModified() const;
 	void _updateTrajConstraints();
 
 	void rcHelpModifyYaw(float &yaw_sp);