Limit horizontal velocity during landing

src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp

@@ -214,7 +214,7 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
 
 		// limit vertical downwards speed (positive z) close to ground
 		// for now we use the altitude above home and assume that we want to land at same height as we took off
-		float vel_limit = math::gradual(_alt_above_ground,
+		float vel_limit = math::gradual(_dist_to_ground,
 						_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
 						_param_mpc_land_speed.get(), _constraints.speed_down);
 

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -52,8 +52,6 @@ bool FlightTaskAutoMapper::update()
 	// always reset constraints because they might change depending on the type
 	_setDefaultConstraints();
 
-	_updateAltitudeAboveGround();
-
 	bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position;
 	bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position;
 
@@ -137,7 +135,7 @@ void FlightTaskAutoMapper::_generateTakeoffSetpoints()
 	_velocity_setpoint = Vector3f(NAN, NAN, NAN);
 
 	// limit vertical speed during takeoff
-	_constraints.speed_up = math::gradual(_alt_above_ground, _param_mpc_land_alt2.get(),
+	_constraints.speed_up = math::gradual(_dist_to_ground, _param_mpc_land_alt2.get(),
 					      _param_mpc_land_alt1.get(), _param_mpc_tko_speed.get(), _constraints.speed_up);
 
 	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
@@ -152,21 +150,6 @@ void FlightTaskAutoMapper::_generateVelocitySetpoints()
 	_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
 }
 
-void FlightTaskAutoMapper::_updateAltitudeAboveGround()
-{
-	// Altitude above ground is by default just the negation of the current local position in D-direction.
-	_alt_above_ground = -_position(2);
-
-	if (PX4_ISFINITE(_dist_to_bottom)) {
-		// We have a valid distance to ground measurement
-		_alt_above_ground = _dist_to_bottom;
-
-	} else if (_sub_home_position.get().valid_alt) {
-		// if home position is set, then altitude above ground is relative to the home position
-		_alt_above_ground = -_position(2) + _sub_home_position.get().z;
-	}
-}
-
 void FlightTaskAutoMapper::updateParams()
 {
 	FlightTaskAuto::updateParams();
@@ -178,5 +161,5 @@ void FlightTaskAutoMapper::updateParams()
 bool FlightTaskAutoMapper::_highEnoughForLandingGear()
 {
 	// return true if altitude is above two meters
-	return _alt_above_ground > 2.0f;
+	return _dist_to_ground > 2.0f;
 }

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -59,11 +59,8 @@ class FlightTaskAutoMapper : public FlightTaskAuto
 	void _generateVelocitySetpoints();
 	void _generateTakeoffSetpoints();
 
-	void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */
 	void updateParams() override; /**< See ModuleParam class */
 
-	float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
-
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
 					(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,

src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@@ -52,8 +52,6 @@ bool FlightTaskAutoMapper2::update()
 	// always reset constraints because they might change depending on the type
 	_setDefaultConstraints();
 
-	_updateAltitudeAboveGround();
-
 	// The only time a thrust set-point is sent out is during
 	// idle. Hence, reset thrust set-point to NAN in case the
 	// vehicle exits idle.
@@ -144,7 +142,7 @@ void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
 {
 	// Takeoff is completely defined by target position
 	_position_setpoint = _target;
-	const float speed_tko = (_alt_above_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up :
+	const float speed_tko = (_dist_to_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up :
 				_param_mpc_tko_speed.get();
 	_velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed
 
@@ -167,21 +165,6 @@ void FlightTaskAutoMapper2::_preparePositionSetpoints()
 	_velocity_setpoint = Vector3f(NAN, NAN, NAN); // No special velocity limitations
 }
 
-void FlightTaskAutoMapper2::_updateAltitudeAboveGround()
-{
-	// Altitude above ground is by default just the negation of the current local position in D-direction.
-	_alt_above_ground = -_position(2);
-
-	if (PX4_ISFINITE(_dist_to_bottom)) {
-		// We have a valid distance to ground measurement
-		_alt_above_ground = _dist_to_bottom;
-
-	} else if (_sub_home_position.get().valid_alt) {
-		// if home position is set, then altitude above ground is relative to the home position
-		_alt_above_ground = -_position(2) + _sub_home_position.get().z;
-	}
-}
-
 void FlightTaskAutoMapper2::updateParams()
 {
 	FlightTaskAuto::updateParams();
@@ -193,7 +176,7 @@ void FlightTaskAutoMapper2::updateParams()
 bool FlightTaskAutoMapper2::_highEnoughForLandingGear()
 {
 	// return true if altitude is above two meters
-	return _alt_above_ground > 2.0f;
+	return _dist_to_ground > 2.0f;
 }
 
 float FlightTaskAutoMapper2::_getLandSpeed()
@@ -209,12 +192,14 @@ float FlightTaskAutoMapper2::_getLandSpeed()
 
 	float speed = 0;
 
-	if (_alt_above_ground > _param_mpc_land_alt1.get()) {
+	if (_dist_to_ground > _param_mpc_land_alt1.get()) {
 		speed = _constraints.speed_down;
 
 	} else {
-		float land_speed = _param_mpc_land_speed.get();
-		float head_room = _constraints.speed_down - land_speed;
+		const float land_speed = 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 head_room = _constraints.speed_down - land_speed;
 
 		speed = land_speed + 2 * (0.5f - throttle) * head_room;
 

src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp

@@ -60,11 +60,8 @@ class FlightTaskAutoMapper2 : public FlightTaskAuto
 	void _prepareTakeoffSetpoints();
 	void _preparePositionSetpoints();
 
-	void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */
 	void updateParams() override; /**< See ModuleParam class */
 
-	float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
-
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
 					(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,

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()
+{
+	// Altitude above ground is local z-position or altitude above home or distance sensor altitude depending on what's available
+	_dist_to_ground = -_position(2);
+
+	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,13 +171,16 @@ class FlightTask : public ModuleParams
 
 	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();
 
 	/** Check and update local position */
 	void _evaluateVehicleLocalPosition();
 
+	void _evaluateDistanceToGround();
+
 	/** Set constraints to default values */
 	virtual void _setDefaultConstraints();
 
@@ -207,7 +211,8 @@ class FlightTask : public ModuleParams
 	matrix::Vector3f _position; /**< current vehicle position */
 	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_bottom = 0.f; /**< 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 @@ class FlightTaskManualAltitude : public FlightTaskManual
 	 */
 	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 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude
 
 	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,17 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f);
 
+/**
+ * Maximum horizontal velocity during landing
+ * Set the value higher than the otherwise expected maximum to disable any slowdown.
+ *
+ * @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,
@@ -682,9 +693,12 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.0f);
 /**
  * Altitude for 1. step of slow landing (descend)
  *
- * 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
+ * Below this altitude:
+ * - descending velocity gets limited to a value
+ * between "MPC_Z_VEL_MAX" and "MPC_LAND_SPEED"
+ * - horizontal velocity gets limited to a value
+ * between "MPC_VEL_MANUAL" and "MPC_LAND_VEL_XY"
+ * for a smooth descent and landing experience.
  * Value needs to be higher than "MPC_LAND_ALT2"
  *
  * @unit m
@@ -698,7 +712,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