ManualPosition: Limit horizontal speed during landing

src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

@@ -33,8 +33,10 @@ bool FlightTask::updateInitialize()
 
 	_sub_vehicle_local_position.update();
 	_sub_attitude.update();
+	_sub_home_position.update();
 
 	_evaluateVehicleLocalPosition();
+	_evaluateDistanceToGround();
 	_checkEkfResetCounters();
 	return true;
 }
@@ -160,6 +162,19 @@ void FlightTask::_evaluateVehicleLocalPosition()
 	}
 }
 
+void FlightTask::_evaluateDistanceToGround()
+{
+	_dist_to_ground = NAN;
+
+	// if there is a valid distance to bottom or vertical distance to home
+	if (PX4_ISFINITE(_dist_to_bottom)) {
+		_dist_to_ground = _dist_to_bottom;
+
+	} else if (_sub_home_position.get().valid_alt) {
+		_dist_to_ground = -(_position(2) - _sub_home_position.get().z);
+	}
+}
+
 void FlightTask::_setDefaultConstraints()
 {
 	_constraints.speed_xy = _param_mpc_xy_vel_max.get();

src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp

@@ -52,6 +52,7 @@
 #include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/home_position.h>
 #include <lib/weather_vane/WeatherVane.hpp>
 
 class FlightTask : public ModuleParams
@@ -170,6 +171,7 @@ protected:
 
 	uORB::SubscriptionData<vehicle_local_position_s>	_sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
 	uORB::SubscriptionData<vehicle_attitude_s>		_sub_attitude{ORB_ID(vehicle_attitude)};
+	uORB::SubscriptionData<home_position_s> 		_sub_home_position{ORB_ID(home_position)};
 
 	/** Reset all setpoints to NAN */
 	void _resetSetpoints();
@@ -177,6 +179,8 @@ protected:
 	/** Check and update local position */
 	void _evaluateVehicleLocalPosition();
 
+	void _evaluateDistanceToGround();
+
 	/** Set constraints to default values */
 	virtual void _setDefaultConstraints();
 
@@ -208,6 +212,7 @@ protected:
 	matrix::Vector3f _velocity; /**< current vehicle velocity */
 	float _yaw = 0.f; /**< current vehicle yaw heading */
 	float _dist_to_bottom = 0.0f; /**< current height above ground level */
+	float _dist_to_ground = 0.f; /**< equals _dist_to_bottom if valid, height above home otherwise */
 
 	/**
 	 * Setpoints which the position controller has to execute.

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -45,8 +45,6 @@ bool FlightTaskManualAltitude::updateInitialize()
 {
 	bool ret = FlightTaskManual::updateInitialize();
 
-	_sub_home_position.update();
-
 	// in addition to manual require valid position and velocity in D-direction and valid yaw
 	return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
 }
@@ -267,22 +265,12 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 
 void FlightTaskManualAltitude::_respectGroundSlowdown()
 {
-	float dist_to_ground = NAN;
-
-	// if there is a valid distance to bottom or vertical distance to home
-	if (PX4_ISFINITE(_dist_to_bottom)) {
-		dist_to_ground = _dist_to_bottom;
-
-	} else if (_sub_home_position.get().valid_alt) {
-		dist_to_ground = -(_position(2) - _sub_home_position.get().z);
-	}
-
 	// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
-	if (PX4_ISFINITE(dist_to_ground)) {
-		const float limit_down = math::gradual(dist_to_ground,
+	if (PX4_ISFINITE(_dist_to_ground)) {
+		const float limit_down = math::gradual(_dist_to_ground,
 						       _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
 						       _param_mpc_land_speed.get(), _constraints.speed_down);
-		const float limit_up = math::gradual(dist_to_ground,
+		const float limit_up = math::gradual(_dist_to_ground,
 						     _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
 						     _param_mpc_tko_speed.get(), _constraints.speed_up);
 		_velocity_setpoint(2) = math::constrain(_velocity_setpoint(2), -limit_up, limit_down);

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -40,7 +40,6 @@
 #pragma once
 
 #include "FlightTaskManual.hpp"
-#include <uORB/topics/home_position.h>
 
 class FlightTaskManualAltitude : public FlightTaskManual
 {
@@ -123,8 +122,6 @@ private:
 	 */
 	void _respectGroundSlowdown();
 
-	uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
-
 	float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
 	float _max_speed_up = 10.0f;

src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -113,6 +113,8 @@ void FlightTaskManualPosition::_scaleSticks()
 		}
 	}
 
+	_velocity_scale = fminf(_computeVelXYGroundDist(), _velocity_scale);
+
 	// scale velocity to its maximum limits
 	Vector2f vel_sp_xy = stick_xy * _velocity_scale;
 
@@ -129,6 +131,20 @@ void FlightTaskManualPosition::_scaleSticks()
 	_velocity_setpoint(1) = vel_sp_xy(1);
 }
 
+float FlightTaskManualPosition::_computeVelXYGroundDist()
+{
+	float max_vel_xy = _constraints.speed_xy;
+
+	// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
+	if (PX4_ISFINITE(_dist_to_ground)) {
+		max_vel_xy = math::gradual(_dist_to_ground,
+					   _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+					   _param_mpc_land_vel_xy.get(), _constraints.speed_xy);
+	}
+
+	return max_vel_xy;
+}
+
 void FlightTaskManualPosition::_updateXYlock()
 {
 	/* If position lock is not active, position setpoint is set to NAN.*/

src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -65,11 +65,13 @@ protected:
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
 					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+					(ParamFloat<px4::params::MPC_LAND_VEL_XY>) _param_mpc_land_vel_xy,
 					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
 					(ParamFloat<px4::params::MPC_ACC_HOR_ESTM>) _param_mpc_acc_hor_estm
 				       )
 private:
+	float _computeVelXYGroundDist();
 	float _velocity_scale{0.0f}; //scales the stick input to velocity
 	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
 

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -350,6 +350,16 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f);
 
+/**
+ * Maximum horizontal velocity during landing
+ *
+ * @unit m/s
+ * @min 0
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_LAND_VEL_XY, 2.f);
+
 /**
  * Enable user assisted descent speed for autonomous land routine.
  * When enabled, descent speed will be equal to MPC_LAND_SPEED at half throttle,
@@ -684,7 +694,9 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.0f);
  *
  * Below this altitude descending velocity gets limited
  * to a value between "MPC_Z_VEL_MAX" and "MPC_LAND_SPEED"
- * to enable a smooth descent experience
+ * to enable a smooth descent experience.
+ * The horizontal velocity also gets limited to a value
+ * between "MPC_VEL_MANUAL" and "MPC_LAND_VEL_XY"
  * Value needs to be higher than "MPC_LAND_ALT2"
  *
  * @unit m
@@ -698,7 +710,8 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f);
 /**
  * Altitude for 2. step of slow landing (landing)
  *
- * Below this altitude descending velocity gets limited to "MPC_LAND_SPEED"
+ * Below this altitude descending and horizontal velocities get
+ * limited to "MPC_LAND_SPEED" and "MPC_LAND_VEL_XY", respectively.
  * Value needs to be lower than "MPC_LAND_ALT1"
  *
  * @unit m