remove some print debugging

hoping that this works at least with invalid local pos but valid
relaxed local pos.

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -743,6 +743,14 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 		!checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
 				     _last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed);
 
+	// !!! Hack for reproducing failure, remove before merge !!!
+	// In nav_state 18 (auto land), pretend to have valid relaxed local pos.
+	if (context.status().nav_state == 18) {
+		// happens anyway when EKF2_GPS_CTRL=0
+		// failsafe_flags.local_position_invalid = true;
+		failsafe_flags.local_position_invalid_relaxed = false;
+	}
+
 	failsafe_flags.local_velocity_invalid =
 		!checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
 				     _last_lvel_fail_time_us, !failsafe_flags.local_velocity_invalid);

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

@@ -266,27 +266,51 @@ 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 land_radius_enabled = _param_mpc_land_radius.get() > 0.0f;
+		const Vector3f pos = _position;  // To avoid it updating in between
+		const bool position_valid = Vector2f(pos).isAllFinite();
 
-		if (PX4_ISFINITE(distance_to_circle)) {
-			max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(),
-					_stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f);
+		float max_speed = INFINITY;
 
-			if (max_speed < 0.5f) {
+		if (position_valid && land_radius_enabled) {
+			const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(pos.xy(), _initial_land_position.xy(),
+							 _param_mpc_land_radius.get(), sticks_ne);
+
+			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 (max_speed < 0.5f) {
+					sticks_xy.setZero();
+				}
+
+			} else {
+				max_speed = 0.f;
 				sticks_xy.setZero();
 			}
-
-		} else {
-			max_speed = 0.f;
-			sticks_xy.setZero();
 		}
 
-		_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);
+		if (position_valid) {
+			_stick_acceleration_xy.setVelocityConstraint(max_speed);
+			_stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, pos,
+					_velocity_setpoint_feedback.xy(), _deltatime);
+			_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
+		} else {
+			PX4_INFO("position invalid branch");
+			// Position independent nudging from FlightTaskDescend.
+			// This cannot take into account any max_speed limit
+			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime);
+			_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
+						_yaw_setpoint);
+
+			// 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;
+		}
+
+		PX4_INFO("_land_position: (%.2f, %.2f)", (double) _land_position(0), (double) _land_position(1));
+		PX4_INFO("_velocity_setpoint: (%.2f, %.2f)", (double) _velocity_setpoint(0), (double) _velocity_setpoint(1));
+		PX4_INFO("_acceleration_setpoint: (%.2f, %.2f)\n", (double) _acceleration_setpoint(0), (double) _acceleration_setpoint(1));
 
 	} 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/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp

@@ -204,6 +204,14 @@ void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector
 	const bool moving = _velocity_setpoint.norm_squared() > FLT_EPSILON;
 	const bool position_locked = Vector2f(_position_setpoint).isAllFinite();
 
+	if (!pos.isAllFinite()) {
+		// position is invalid. We may still enter here through a mode
+		// requiring only relaxed local position. In this case, no
+		// position locking can happen, so we give no position setpoint.
+		_position_setpoint.setNaN();
+		return;
+	}
+
 	// lock position
 	if (!moving && !position_locked) {
 		_position_setpoint = pos.xy();

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -245,6 +245,13 @@ bool PositionControl::_inputValid()
 			valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i));
 		}
 	}
+	if (!valid) {
+
+		printf("invalidated with:\n_pos_sp: (%.2f, %.2f, %.2f)\n", (double) _pos_sp(0), (double) _pos_sp(1), (double) _pos_sp(2));
+		printf("_vel_sp: (%.2f, %.2f, %.2f)\n", (double) _vel_sp(0), (double) _vel_sp(1), (double) _vel_sp(2));
+		printf("_acc_sp: (%.2f, %.2f, %.2f)\n", (double) _acc_sp(0), (double) _acc_sp(1), (double) _acc_sp(2));
+
+	}
 
 	return valid;
 }

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. Nudging is only limited 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);