MC auto: add maximum RC assist distance during landing

src/lib/mathlib/math/TrajMath.hpp

@@ -110,5 +110,44 @@ inline float computeBrakingDistanceFromVelocity(const float velocity, const floa
 	return velocity * (velocity / (2.0f * accel) + accel_delay_max / jerk);
 }
 
+/* Compute the maximum distance between a point and a circle given a direction vector pointing from the point
+ * towards the circle. The point can be inside or outside the circle.
+ *                  _
+ *               ,=' '=,               __
+ *    P-->------/-------A   Distance = PA
+ *       Dir   |    x    |
+ *              \       /
+ *               "=,_,="
+ * Equation to solve: ||(point - circle_pos) + direction_unit * distance_to_circle|| = radius
+ *
+ * @param pos position of the point
+ * @param circle_pos position of the center of the circle
+ * @param radius radius of the circle
+ * @param direction vector pointing from the point towards the circle
+ *
+ * @return longest distance between the point to the circle in the direction indicated by the vector or NAN if the
+ * vector does not point towards the circle
+ */
+inline float getMaxDistanceToCircle(const matrix::Vector2f &pos, const matrix::Vector2f &circle_pos, float radius,
+				    const matrix::Vector2f &direction)
+{
+	matrix::Vector2f center_to_pos = pos - circle_pos;
+	const float b = 2.f * center_to_pos.dot(direction.unit_or_zero());
+	const float c = center_to_pos.norm_squared() - radius * radius;
+	const float delta = b * b - 4.f * c;
+
+	float distance_to_circle;
+
+	if (delta >= 0.f && direction.longerThan(0.f)) {
+		distance_to_circle = fmaxf((-b + sqrtf(delta)) / 2.f, 0.f);
+
+	} else {
+		// Never intersecting the circle
+		distance_to_circle = NAN;
+	}
+
+	return distance_to_circle;
+}
+
 } /* namespace traj */
 } /* namespace math */

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

@@ -229,9 +229,10 @@ void FlightTaskAuto::_prepareLandSetpoints()
 	}
 
 	if (_type_previous != WaypointType::land) {
-		// initialize yaw
+		// initialize yaw and xy-position
 		_land_heading = _yaw_setpoint;
 		_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
+		_initial_land_position = Vector3f(_target(0), _target(1), NAN);
 	}
 
 	// Update xy-position in case of landing position changes (etc. precision landing)
@@ -248,7 +249,29 @@ void FlightTaskAuto::_prepareLandSetpoints()
 						       _deltatime);
 		}
 
-		_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _land_heading, _position,
+		Vector2f sticks_xy = _sticks.getPitchRollExpo();
+		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;
+
+		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();
+		}
+
+		_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);
 

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

@@ -166,6 +166,7 @@ protected:
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
 					(ParamFloat<px4::params::MPC_LAND_CRWL>) _param_mpc_land_crawl_speed,
 					(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
+					(ParamFloat<px4::params::MPC_LAND_RADIUS>) _param_mpc_land_radius,
 					(ParamFloat<px4::params::MPC_LAND_ALT1>)
 					_param_mpc_land_alt1, // altitude at which we start ramping down speed
 					(ParamFloat<px4::params::MPC_LAND_ALT2>)
@@ -201,6 +202,8 @@ private:
 
 	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 
+	matrix::Vector3f _initial_land_position;
+
 	void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */
 	bool _evaluateTriplets(); /**< Checks and sets triplets. */
 	bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp

@@ -75,7 +75,8 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
 {
 	// maximum commanded acceleration and velocity
 	Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get());
-	Vector2f velocity_scale(_param_mpc_vel_manual.get(), _param_mpc_vel_manual.get());
+	const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint);
+	Vector2f velocity_scale(velocity_sc, velocity_sc);
 
 	acceleration_scale *= 2.f; // because of drag the average acceleration is half
 

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp

@@ -60,6 +60,9 @@ public:
 	void generateSetpoints(matrix::Vector2f stick_xy, const float yaw, const float yaw_sp, const matrix::Vector3f &pos,
 			       const matrix::Vector2f &vel_sp_feedback, const float dt);
 	void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp);
+	float getMaxAcceleration() { return _param_mpc_acc_hor.get(); };
+	float getMaxJerk() { return _param_mpc_jerk_max.get(); };
+	void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); };
 
 private:
 	void applyJerkLimit(const float dt);
@@ -79,6 +82,8 @@ private:
 	matrix::Vector2f _acceleration_setpoint;
 	matrix::Vector2f _acceleration_setpoint_prev;
 
+	float _velocity_constraint{INFINITY};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
 		(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -478,6 +478,20 @@ PARAM_DEFINE_FLOAT(MPC_LAND_CRWL, 0.3f);
  */
 PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0);
 
+/**
+ * User assisted landing radius
+ *
+ * When user assisted descent is enabled (see MPC_LAND_RC_HELP),
+ * this parameter controls the maximum position adjustment
+ * allowed from the original landing point.
+ *
+ * @unit m
+ * @min 0
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, 1000.f);
+
 /**
  * Takeoff climb rate
  *