diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
index 98e78768c4..e2b52c6880 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
@@ -47,10 +47,16 @@ namespace ControlMath
 void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode,
 		      vehicle_attitude_setpoint_s &att_sp)
 {
-	if (omni_att_mode > 0) {
-		thrustToOmniAttitude(thr_sp, yaw_sp, att_sp);
+	if (omni_att_mode > 2 || omni_att_mode < 0) {
+		PX4_ERR("omni_att_mode set to unknown value!");
+	}
 
-	} else {
+	switch (omni_att_mode) {
+	case 2: // Attitude is set to the fixed zero roll and pitch (used for omnidirectional vehicles)
+		thrustToZeroTiltAttitude(thr_sp, yaw_sp, att_sp);
+		break;
+
+	default:
 		bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
 		att_sp.thrust_body[2] = -thr_sp.length();
 	}
@@ -109,7 +115,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
 	att_sp.yaw_body = euler(2);
 }
 
-void thrustToOmniAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
+void thrustToZeroTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
 {
 	// set Z axis to upward direction
 	Vector3f body_z = Vector3f(0.f, 0.f, 1.f);
diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
index 42598654a8..6b83718462 100644
--- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
+++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
@@ -63,12 +63,12 @@ void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const
 void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
 
 /**
- * Converts thrust vector and yaw set-point to a desired attitude for an omni-directional multirotor.
+ * Converts thrust vector and yaw set-point to a zero-tilt attitude for an omni-directional multirotor.
  * @param thr_sp a 3D vector
  * @param yaw_sp the desired yaw
  * @param att_sp attitude setpoint to fill
  */
-void thrustToOmniAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
+void thrustToZeroTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
 
 /**
  * Outputs the sum of two vectors but respecting the limits and priority.