mc_pos_control: add method to constrain velocity setpoint

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -381,6 +381,8 @@ private:
 
 	float get_vel_close(const matrix::Vector2f &unit_prev_to_current, const matrix::Vector2f &unit_current_to_next);
 
+	void constrain_velocity_setpoint();
+
 	void set_manual_acceleration_xy(matrix::Vector2f &stick_input_xy_NED);
 
 	void set_manual_acceleration_z(float &max_acc_z, const float stick_input_z_NED);
@@ -965,6 +967,22 @@ MulticopterPositionControl::get_vel_close(const matrix::Vector2f &unit_prev_to_c
 
 }
 
+void
+MulticopterPositionControl::constrain_velocity_setpoint()
+{
+
+	/* make sure velocity setpoint is constrained in all directions (xyz) */
+	float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
+
+	if (vel_norm_xy > _vel_max_xy) {
+		_vel_sp(0) = _vel_sp(0) * _vel_max_xy / vel_norm_xy;
+		_vel_sp(1) = _vel_sp(1) * _vel_max_xy / vel_norm_xy;
+	}
+
+	_vel_sp(2) = math::constrain(_vel_sp(2), -_vel_max_up.get(), _vel_max_down.get());
+
+}
+
 float
 MulticopterPositionControl::get_cruising_speed_xy()
 {
@@ -2461,19 +2479,13 @@ MulticopterPositionControl::calculate_velocity_setpoint()
 		set_takeoff_velocity(_vel_sp(2));
 	}
 
+	constrain_velocity_setpoint();
 	/* make sure velocity setpoint is constrained in all directions (xyz) */
 	float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
 
 	/* check if the velocity demand is significant */
 	_vel_sp_significant =  vel_norm_xy > 0.5f * _vel_max_xy;
 
-	if (vel_norm_xy > _vel_max_xy) {
-		_vel_sp(0) = _vel_sp(0) * _vel_max_xy / vel_norm_xy;
-		_vel_sp(1) = _vel_sp(1) * _vel_max_xy / vel_norm_xy;
-	}
-
-	_vel_sp(2) = math::constrain(_vel_sp(2), -_vel_max_up.get(), _vel_max_down.get());
-
 	_vel_sp_prev = _vel_sp;
 }