diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
index 2217b46cd2..98e78768c4 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
@@ -44,9 +44,10 @@ using namespace matrix;
 
 namespace ControlMath
 {
-void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp, const bool omni)
+void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode,
+		      vehicle_attitude_setpoint_s &att_sp)
 {
-	if (omni) {
+	if (omni_att_mode > 0) {
 		thrustToOmniAttitude(thr_sp, yaw_sp, att_sp);
 
 	} else {
diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
index cf5819d697..42598654a8 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
@@ -50,9 +50,10 @@ namespace ControlMath
  * @param thr_sp desired 3D thrust vector
  * @param yaw_sp the desired yaw
  * @param att_sp attitude setpoint to fill
+ * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-daisy-chain 2-zero-tilt
  */
-void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp,
-		      const bool omni = true);
+void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode,
+		      vehicle_attitude_setpoint_s &att_sp);
 /**
  * Converts a body z vector and yaw set-point to a desired attitude.
  * @param body_z a world frame 3D vector in direction of the desired body z axis
diff --git a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
index 2623ba69f1..5e7dc3a434 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
@@ -45,8 +45,9 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	 * reason: thrust pointing full upward */
 	Vector3f thr{0.f, 0.f, -1.f};
 	float yaw = 0.f;
+	int omni_att_mode = 0;
 	vehicle_attitude_setpoint_s att{};
-	thrustToAttitude(thr, yaw, att);
+	thrustToAttitude(thr, yaw, omni_att_mode, att);
 	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
 	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
 	EXPECT_FLOAT_EQ(att.yaw_body, 0.f);
@@ -55,7 +56,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	/* expected: same as before but with 90 yaw
 	 * reason: only yaw changed */
 	yaw = M_PI_2_F;
-	thrustToAttitude(thr, yaw, att);
+	thrustToAttitude(thr, yaw, omni_att_mode, att);
 	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
 	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
 	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
@@ -65,7 +66,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	 * reason: thrust points straight down and order Euler
 	 * order is: 1. roll, 2. pitch, 3. yaw */
 	thr = Vector3f(0.f, 0.f, 1.f);
-	thrustToAttitude(thr, yaw, att);
+	thrustToAttitude(thr, yaw, omni_att_mode, att);
 	EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F);
 	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
 	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
index ff67df4b61..69999c9313 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -361,8 +361,8 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 	_thr_sp.copyTo(local_position_setpoint.thrust);
 }
 
-void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
+void PositionControl::getAttitudeSetpoint(const int omni_att_mode, vehicle_attitude_setpoint_s &attitude_setpoint) const
 {
-	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
+	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, omni_att_mode, attitude_setpoint);
 	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
 }
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
index 07cf0b03f6..185edde273 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
@@ -169,9 +169,10 @@ public:
 	 * Get the controllers output attitude setpoint
 	 * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control.
 	 * It needs to be executed by the attitude controller to achieve velocity and position tracking.
+	 * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-daisy-chain 2-zero-tilt
 	 * @param attitude_setpoint reference to struct to fill up
 	 */
-	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
+	void getAttitudeSetpoint(const int omni_att_mode, vehicle_attitude_setpoint_s &attitude_setpoint) const;
 
 private:
 	/**
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 6a2399a823..dc798dd320 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -656,7 +656,7 @@ MulticopterPositionControl::Run()
 
 			vehicle_attitude_setpoint_s attitude_setpoint{};
 			attitude_setpoint.timestamp = time_stamp_now;
-			_control.getAttitudeSetpoint(attitude_setpoint);
+			_control.getAttitudeSetpoint(_param_mc_omni_mode.get(), attitude_setpoint);
 
 			// Part of landing logic: if ground-contact/maybe landed was detected, turn off
 			// controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.