diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt
index 6a75028b01..a6394aced8 100644
--- a/src/modules/mc_pos_control/CMakeLists.txt
+++ b/src/modules/mc_pos_control/CMakeLists.txt
@@ -32,6 +32,7 @@
 ############################################################################
 
 add_subdirectory(Takeoff)
+add_subdirectory(PositionControl)
 
 px4_add_module(
 	MODULE modules__mc_pos_control
@@ -40,9 +41,8 @@ px4_add_module(
 		-Wno-implicit-fallthrough # TODO: fix and remove
 	SRCS
 		mc_pos_control_main.cpp
-		PositionControl.cpp
-		Utility/ControlMath.cpp
 	DEPENDS
+		PositionControl
 		controllib
 		FlightTasks
 		git_ecl
diff --git a/src/modules/mc_pos_control/PositionControl/CMakeLists.txt b/src/modules/mc_pos_control/PositionControl/CMakeLists.txt
new file mode 100644
index 0000000000..d9012e5a92
--- /dev/null
+++ b/src/modules/mc_pos_control/PositionControl/CMakeLists.txt
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(PositionControl
+	PositionControl.cpp
+	ControlMath.cpp
+)
+target_include_directories(PositionControl
+	PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}
+)
diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp
similarity index 100%
rename from src/modules/mc_pos_control/Utility/ControlMath.cpp
rename to src/modules/mc_pos_control/PositionControl/ControlMath.cpp
diff --git a/src/modules/mc_pos_control/Utility/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp
similarity index 100%
rename from src/modules/mc_pos_control/Utility/ControlMath.hpp
rename to src/modules/mc_pos_control/PositionControl/ControlMath.hpp
diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
similarity index 99%
rename from src/modules/mc_pos_control/PositionControl.cpp
rename to src/modules/mc_pos_control/PositionControl/PositionControl.cpp
index 4808efabe9..5e2b57fa5a 100644
--- a/src/modules/mc_pos_control/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -38,7 +38,7 @@
 #include "PositionControl.hpp"
 #include <float.h>
 #include <mathlib/mathlib.h>
-#include "Utility/ControlMath.hpp"
+#include <ControlMath.hpp>
 #include <px4_defines.h>
 
 using namespace matrix;
diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
similarity index 100%
rename from src/modules/mc_pos_control/PositionControl.hpp
rename to src/modules/mc_pos_control/PositionControl/PositionControl.hpp
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 c0bfbd8f82..c60e6f2a8d 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 - 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 - 2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -68,7 +68,7 @@
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 
 #include "PositionControl.hpp"
-#include "Utility/ControlMath.hpp"
+#include "PositionControl/ControlMath.hpp"
 #include "Takeoff.hpp"
 
 #include <float.h>
diff --git a/src/systemcmds/tests/test_controlmath.cpp b/src/systemcmds/tests/test_controlmath.cpp
index 8fc69f8a25..ba2fd4f003 100644
--- a/src/systemcmds/tests/test_controlmath.cpp
+++ b/src/systemcmds/tests/test_controlmath.cpp
@@ -37,7 +37,7 @@
  */
 
 #include <unit_test.h>
-#include <mc_pos_control/Utility/ControlMath.hpp>
+#include <mc_pos_control/PositionControl/ControlMath.hpp>
 #include <mathlib/mathlib.h>
 #include <float.h>