diff --git a/src/lib/motion_planning/CMakeLists.txt b/src/lib/motion_planning/CMakeLists.txt
index 0d1fa8546c..f913c495d6 100644
--- a/src/lib/motion_planning/CMakeLists.txt
+++ b/src/lib/motion_planning/CMakeLists.txt
@@ -34,6 +34,8 @@
 px4_add_library(motion_planning
 	VelocitySmoothing.cpp
 	PositionSmoothing.cpp
+	ManualVelocitySmoothingXY.cpp
+	ManualVelocitySmoothingZ.cpp
 )
 
 target_include_directories(motion_planning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
diff --git a/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.cpp b/src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
similarity index 100%
rename from src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.cpp
rename to src/lib/motion_planning/ManualVelocitySmoothingXY.cpp
diff --git a/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.hpp b/src/lib/motion_planning/ManualVelocitySmoothingXY.hpp
similarity index 100%
rename from src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.hpp
rename to src/lib/motion_planning/ManualVelocitySmoothingXY.hpp
diff --git a/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXYTest.cpp b/src/lib/motion_planning/ManualVelocitySmoothingXYTest.cpp
similarity index 98%
rename from src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXYTest.cpp
rename to src/lib/motion_planning/ManualVelocitySmoothingXYTest.cpp
index e66218377b..05f19dcf39 100644
--- a/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXYTest.cpp
+++ b/src/lib/motion_planning/ManualVelocitySmoothingXYTest.cpp
@@ -39,7 +39,7 @@
 #include <gtest/gtest.h>
 #include <matrix/matrix/math.hpp>
 
-#include "ManualVelocitySmoothingXY.hpp"
+#include <motion_planning/ManualVelocitySmoothingXY.hpp>
 
 using namespace matrix;
 
diff --git a/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.cpp b/src/lib/motion_planning/ManualVelocitySmoothingZ.cpp
similarity index 100%
rename from src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.cpp
rename to src/lib/motion_planning/ManualVelocitySmoothingZ.cpp
diff --git a/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.hpp b/src/lib/motion_planning/ManualVelocitySmoothingZ.hpp
similarity index 100%
rename from src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.hpp
rename to src/lib/motion_planning/ManualVelocitySmoothingZ.hpp
diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp
index f04f3a7c55..b7acb085dc 100644
--- a/src/lib/tecs/TECS.hpp
+++ b/src/lib/tecs/TECS.hpp
@@ -47,7 +47,7 @@
 #include <uORB/topics/tecs_status.h>
 #include <uORB/uORB.h>
 #include <motion_planning/VelocitySmoothing.hpp>
-#include <flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.hpp>
+#include <motion_planning/ManualVelocitySmoothingZ.hpp>
 
 class TECS
 {
diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
index 056c2e5139..dabbac620d 100644
--- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
+++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
@@ -40,7 +40,7 @@
 #pragma once
 
 #include "FlightTaskManualAltitude.hpp"
-#include "ManualVelocitySmoothingZ.hpp"
+#include <motion_planning/ManualVelocitySmoothingZ.hpp>
 
 class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
 {
diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
index fbf58c2745..12804b59f2 100644
--- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
+++ b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
@@ -40,8 +40,8 @@
 #pragma once
 
 #include "FlightTaskManualPosition.hpp"
-#include "ManualVelocitySmoothingXY.hpp"
-#include "ManualVelocitySmoothingZ.hpp"
+#include <motion_planning/ManualVelocitySmoothingXY.hpp>
+#include <motion_planning/ManualVelocitySmoothingZ.hpp>
 
 using matrix::Vector2f;
 using matrix::Vector3f;
diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt
index a1d7c5bb50..daa0a62bf4 100644
--- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt
+++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt
@@ -33,8 +33,6 @@
 
 px4_add_library(FlightTaskUtility
 	StraightLine.cpp
-	ManualVelocitySmoothingXY.cpp
-	ManualVelocitySmoothingZ.cpp
 	Sticks.cpp
 	StickAccelerationXY.cpp
 	StickYaw.cpp
@@ -42,5 +40,3 @@ px4_add_library(FlightTaskUtility
 
 target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning)
 target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-px4_add_unit_gtest(SRC ManualVelocitySmoothingXYTest.cpp LINKLIBS FlightTaskUtility)