Fix max-hagl restriction to position/altitude control (#23667)

* fix max-hagl restriction to position/altitude control

* max hagl vel restriction in ManAcc position mode

* use interpolate func, change naming

* simplyfied vertical vel limitation

* move velocity-constraint adjustment to StickAccelXY

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -59,8 +59,29 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se
 
 bool FlightTaskManualAcceleration::update()
 {
+	const vehicle_local_position_s vehicle_local_pos = _sub_vehicle_local_position.get();
+	setMaxDistanceToGround(vehicle_local_pos.hagl_max_xy);
 	bool ret = FlightTaskManualAltitudeSmoothVel::update();
 
+	float max_hagl_ratio = 0.0f;
+
+	if (PX4_ISFINITE(vehicle_local_pos.hagl_max_xy) && vehicle_local_pos.hagl_max_xy > FLT_EPSILON) {
+		max_hagl_ratio = (vehicle_local_pos.dist_bottom) / vehicle_local_pos.hagl_max_xy;
+	}
+
+	// limit horizontal velocity near max hagl to decrease chance of larger gound distance jumps
+	static constexpr float factor_threshold = 0.8f; // threshold ratio of max_hagl
+	static constexpr float min_vel = 2.f; // minimum max-velocity near max_hagl
+
+	if (max_hagl_ratio > factor_threshold) {
+		max_hagl_ratio = math::min(max_hagl_ratio, 1.f);
+		const float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get());
+		_stick_acceleration_xy.setVelocityConstraint(interpolate(vxy_max, factor_threshold, min_vel, vxy_max, min_vel));
+
+	} else {
+		_stick_acceleration_xy.setVelocityConstraint(math::min(_param_mpc_vel_manual.get(), vehicle_local_pos.vxy_max));
+	}
+
 	_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position,
 			_velocity_setpoint_feedback.xy(), _deltatime);
 	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -52,4 +52,9 @@ protected:
 
 	StickAccelerationXY _stick_acceleration_xy{this};
 	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor
+				       )
 };

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -81,11 +81,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 		_min_distance_to_ground = -INFINITY;
 	}
 
-	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) {
-		_max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max;
-
-	} else {
-		_max_distance_to_ground = INFINITY;
+	if (!PX4_ISFINITE(_max_distance_to_ground) && PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max_z)) {
+		_max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max_z;
 	}
 }
 
@@ -154,8 +151,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 	if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) {
 		// terrain following
 		_terrainFollowing(apply_brake, stopped);
-		// respect maximum altitude
-		_respectMaxAltitude();
 
 	} else {
 		// normal mode where height is dependent on local frame
@@ -185,9 +180,10 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 			// user demands velocity change
 			_position_setpoint(2) = NAN;
 			// ensure that maximum altitude is respected
-			_respectMaxAltitude();
 		}
 	}
+
+	_respectMaxAltitude();
 }
 
 void FlightTaskManualAltitude::_respectMinAltitude()
@@ -229,29 +225,20 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 {
 	if (PX4_ISFINITE(_dist_to_bottom)) {
 
-		// if there is a valid maximum distance to ground, linearly increase speed limit with distance
-		// below the maximum, preserving control loop vertical position error gain.
-		// TODO: manipulate the velocity setpoint instead of tweaking the saturation of the controller
+		float vel_constrained = _param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom);
+
 		if (PX4_ISFINITE(_max_distance_to_ground)) {
-			_constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom),
-								-_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get());
+			_constraints.speed_up = math::constrain(vel_constrained, -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get());
 
 		} else {
 			_constraints.speed_up = _param_mpc_z_vel_max_up.get();
 		}
 
-		// if distance to bottom exceeded maximum distance, slowly approach maximum distance
-		if (_dist_to_bottom > _max_distance_to_ground) {
-			// difference between current distance to ground and maximum distance to ground
-			const float delta_distance_to_max = _dist_to_bottom - _max_distance_to_ground;
-			// set position setpoint to maximum distance to ground
-			_position_setpoint(2) = _position(2) + delta_distance_to_max;
-			// limit speed downwards to 0.7m/s
-			_constraints.speed_down = math::min(_param_mpc_z_vel_max_dn.get(), 0.7f);
-
-		} else {
-			_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
+		if (_dist_to_bottom > _max_distance_to_ground && !(_sticks.getThrottleZeroCenteredExpo() < FLT_EPSILON)) {
+			_velocity_setpoint(2) = math::constrain(-vel_constrained, 0.f, _param_mpc_z_vel_max_dn.get());
 		}
+
+		_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
 	}
 }
 
@@ -306,6 +293,7 @@ bool FlightTaskManualAltitude::update()
 	_scaleSticks();
 	_updateSetpoints();
 	_constraints.want_takeoff = _checkTakeoff();
+	_max_distance_to_ground = INFINITY;
 
 	return ret;
 }

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -53,6 +53,7 @@ public:
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;
+	void setMaxDistanceToGround(float max_distance) { _max_distance_to_ground = max_distance; }
 
 protected:
 	void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */

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

@@ -73,8 +73,20 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration
 void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
 		const matrix::Vector2f &vel_sp_feedback, const float dt)
 {
+	// gradually adjust velocity constraint because good tracking is required for the drag estimation
+	if (fabsf(_targeted_velocity_constraint - _current_velocity_constraint) > 0.1f) {
+		if (!PX4_ISFINITE(_current_velocity_constraint) || !PX4_ISFINITE(_targeted_velocity_constraint)) {
+			_current_velocity_constraint = _targeted_velocity_constraint;
+
+		} else {
+			_current_velocity_constraint = math::constrain(_targeted_velocity_constraint,
+						       _current_velocity_constraint - dt * _param_mpc_acc_hor.get(),
+						       _current_velocity_constraint + dt * _param_mpc_acc_hor.get());
+		}
+	}
+
 	// maximum commanded velocity can be constrained dynamically
-	const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint);
+	const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _current_velocity_constraint);
 	Vector2f velocity_scale(velocity_sc, velocity_sc);
 	// maximum commanded acceleration is scaled down with velocity
 	const float acceleration_sc = _param_mpc_acc_hor.get() * (velocity_sc / _param_mpc_vel_manual.get());

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

@@ -63,7 +63,8 @@ public:
 	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); };
+	void setVelocityConstraint(float vel) { _targeted_velocity_constraint = fmaxf(vel, FLT_EPSILON); };
+	float getVelocityConstraint() { return _current_velocity_constraint; };
 
 private:
 	CollisionPrevention _collision_prevention{this};
@@ -85,7 +86,8 @@ private:
 	matrix::Vector2f _acceleration_setpoint;
 	matrix::Vector2f _acceleration_setpoint_prev;
 
-	float _velocity_constraint{INFINITY};
+	float _targeted_velocity_constraint{INFINITY};
+	float _current_velocity_constraint{INFINITY};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,