diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 827118156b..fb29e0d6f3 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -217,6 +217,44 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
 	attitude_setpoint.q_d_valid = true;
 
 	attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
+
+	///////// Added for Omni ////////////////
+	// set the euler angles, for logging only, must not be used for control
+	attitude_setpoint.roll_body = 0;
+	attitude_setpoint.pitch_body = 0;
+
+	Quatf q_sp_omni = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
+	q_sp_omni.copyTo(attitude_setpoint.q_d);
+	attitude_setpoint.q_d_valid = true;
+
+	float x_thrust = _manual_control_sp.x;
+	float y_thrust = _manual_control_sp.y;
+	float z_thrust = _manual_control_sp.z;
+
+	// Check if the total horizontal thrust has exceeded the maximum
+	Vector2f h_thrust = Vector2f(x_thrust, y_thrust);
+	float h_thrust_norm = h_thrust.norm();
+
+	if (h_thrust_norm > 1.0F) {
+		h_thrust /= h_thrust_norm;
+	}
+
+	const float max_xy_thrust_ratio = 0.3;
+	h_thrust *= max_xy_thrust_ratio;
+
+	// Check if the total thrust has exceeded the maximum
+	Vector3f total_thrust = Vector3f(h_thrust(0), h_thrust(1), z_thrust);
+	float total_thrust_norm = total_thrust.norm();
+
+	if (total_thrust_norm > 1.0F) {
+		total_thrust /= total_thrust_norm;
+	}
+
+	attitude_setpoint.thrust_body[0] = throttle_curve(total_thrust(0));
+	attitude_setpoint.thrust_body[1] = throttle_curve(total_thrust(1));
+	attitude_setpoint.thrust_body[2] = -throttle_curve(total_thrust(2));
+	///////// End of added for omni /////////////
+
 	attitude_setpoint.timestamp = hrt_absolute_time();
 
 	_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);