initial fix: param -1 to turn off, also turn off if position invalid, where turning off = nudging from descend

problems:
 - if disabled only due to param -1, this gives acceleration commands
   but they do not translate to the drone moving. what is missing?
 - if disabled due to invalid local pos, the setpoints are rejected by
   mpc as invalid.

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -266,27 +266,54 @@ void FlightTaskAuto::_prepareLandSetpoints()
 		Vector2f sticks_ne = sticks_xy;
 		Sticks::rotateIntoHeadingFrameXY(sticks_ne, _yaw, _land_heading);
 
-		const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(),
-						 _param_mpc_land_radius.get(), sticks_ne);
-		float max_speed;
+		const bool position_valid = !isnan(_position(0)) && !isnan(_position(1));
 
-		if (PX4_ISFINITE(distance_to_circle)) {
-			max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(),
-					_stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f);
+		if (_param_mpc_land_radius.get() > 0 && position_valid) {
 
-			if (max_speed < 0.5f) {
+			PX4_INFO(" fancy nudging");
+
+			// Only allow nudging if it points towards of the circle defined by MPC_LAND_RADIUS
+			// This is only allowed for valid local position.
+
+			const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(),
+							_param_mpc_land_radius.get(), sticks_ne);
+			float max_speed;
+
+
+			if (PX4_ISFINITE(distance_to_circle)) {
+				PX4_INFO("  circle distance finite");
+				max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(),
+						_stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f);
+
+				if (max_speed < 0.5f) {
+					sticks_xy.setZero();
+				}
+
+			} else {
+				max_speed = 0.f;
 				sticks_xy.setZero();
 			}
 
-		} else {
-			max_speed = 0.f;
-			sticks_xy.setZero();
-		}
+			PX4_INFO(" max speed: %.2f", (double) max_speed);
 
-		_stick_acceleration_xy.setVelocityConstraint(max_speed);
-		_stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position,
-				_velocity_setpoint_feedback.xy(), _deltatime);
-		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
+			_stick_acceleration_xy.setVelocityConstraint(max_speed);
+			_stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position,
+					_velocity_setpoint_feedback.xy(), _deltatime);
+			_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
+		} else {
+			// Nudging unrestricted, also works for invalid local pos
+			PX4_INFO(" basic nudging");
+			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime);
+			_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
+						_yaw_setpoint);
+			PX4_INFO(" accel sp: (%.2f, %.2f)", (double) _acceleration_setpoint(0), (double) _acceleration_setpoint(1));
+
+
+			// Stick full up -1 -> stop, stick full down 1 -> double the value
+			_velocity_setpoint(2) *= (1 - _sticks.getThrottleZeroCenteredExpo());
+			_acceleration_setpoint(2) -= _sticks.getThrottleZeroCentered() * 10.f;
+
+		}
 
 	} else {
 		// Make sure we have a valid land position even in the case we loose RC while amending it

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -53,6 +53,7 @@
 #include <lib/weather_vane/WeatherVane.hpp>
 #include "Sticks.hpp"
 #include "StickAccelerationXY.hpp"
+#include "StickTiltXY.hpp"
 
 /**
  * This enum has to agree with position_setpoint_s type definition
@@ -145,6 +146,7 @@ protected:
 	Vector3f _unsmoothed_velocity_setpoint;
 	Sticks _sticks{this};
 	StickAccelerationXY _stick_acceleration_xy{this};
+	StickTiltXY _stick_tilt_xy{this};
 	StickYaw _stick_yaw{this};
 	matrix::Vector3f _land_position;
 	float _land_heading;

src/modules/mc_pos_control/multicopter_nudging_params.c

@@ -56,10 +56,12 @@ PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0);
  * When nudging is enabled (see MPC_LAND_RC_HELP), this controls
  * the maximum allowed horizontal displacement from the original landing point.
  *
+ * Setting the radius to -1 will disable this feature. Only works while local position is valid.
+ *
  * @unit m
- * @min 0
+ * @min -1
  * @decimal 0
  * @increment 1
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, 1000.f);
+PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, -1.0f);