move velocity-constraint adjustment to StickAccelXY

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

@@ -65,28 +65,18 @@ bool FlightTaskManualAcceleration::update()
 
 	float max_hagl_ratio = 0.0f;
 
-	if (PX4_ISFINITE(vehicle_local_pos.hagl_max_xy)) {
+	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
-	const float factor_threshold = 0.8f; // threshold ratio of max_hagl
-	const float min_vel = 2.f; // minimum max-velocity near max_hagl
+	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);
-		float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get());
-
-		vxy_max = interpolate(vxy_max, factor_threshold, min_vel, vxy_max, min_vel);
-
-		const float current_vel_constraint = _stick_acceleration_xy.getVelocityConstraint();
-
-		if (fabsf(current_vel_constraint - vxy_max) > 0.1f) {
-			// gradually adjust velocity constraint because good tracking is required for the drag estimation
-			const float v_limit = math::constrain(vxy_max, current_vel_constraint - _deltatime * _param_mpc_acc_hor.get(),
-							      current_vel_constraint + _deltatime * _param_mpc_acc_hor.get());
-			_stick_acceleration_xy.setVelocityConstraint(v_limit);
-		}
+		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));

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,8 +63,8 @@ class StickAccelerationXY : public ModuleParams
 	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); };
-	float getVelocityConstraint() { return _velocity_constraint; };
+	void setVelocityConstraint(float vel) { _targeted_velocity_constraint = fmaxf(vel, FLT_EPSILON); };
+	float getVelocityConstraint() { return _current_velocity_constraint; };
 
 private:
 	CollisionPrevention _collision_prevention{this};
@@ -86,7 +86,8 @@ class StickAccelerationXY : public ModuleParams
 	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,