add max vel constraints to multiplatform MPC

src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp

@@ -647,6 +647,21 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 
 			_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
 
+			/* make sure velocity setpoint is saturated in xy*/
+			float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) +
+				_vel_sp(1)*_vel_sp(1));
+			if (vel_norm_xy > _params.vel_max(0)) {
+				/* note assumes vel_max(0) == vel_max(1) */
+				_vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy;
+				_vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy;
+			}
+
+			/* make sure velocity setpoint is saturated in z*/
+			float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2));
+			if (vel_norm_z > _params.vel_max(2)) {
+				_vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z;
+			}
+
 			if (!_control_mode->data().flag_control_altitude_enabled) {
 				_reset_alt_sp = true;
 				_vel_sp(2) = 0.0f;