diff --git a/src/modules/motion.h b/src/modules/motion.h
index 30ae21f..f9c560e 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -239,13 +239,13 @@ public:
 
     /// @returns true if all planned moves have been finished for one axis
     /// @param axis requested
-    bool QueueEmpty(Axis axis) const
-#ifndef UNITTEST
-    {
+#if !defined(UNITTEST) || defined(UNITTEST_MOTION)
+    bool QueueEmpty(Axis axis) const {
         return axisData[axis].ctrl.QueueEmpty();
     }
 #else
-        ;
+    // Force STUB for testing
+    bool QueueEmpty(Axis axis) const;
 #endif
 
     /// @returns false if new moves can still be planned for one axis
diff --git a/tests/unit/modules/motion/CMakeLists.txt b/tests/unit/modules/motion/CMakeLists.txt
index 536a799..5453640 100644
--- a/tests/unit/modules/motion/CMakeLists.txt
+++ b/tests/unit/modules/motion/CMakeLists.txt
@@ -1,4 +1,5 @@
 # common include and source directories
+add_compile_definitions(UNITTEST_MOTION)
 set(include_common ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal)
 set(source_common
     ${CMAKE_SOURCE_DIR}/src/modules/motion.cpp
diff --git a/tests/unit/modules/motion/test_motion.cpp b/tests/unit/modules/motion/test_motion.cpp
index 03c98ce..3871ab8 100644
--- a/tests/unit/modules/motion/test_motion.cpp
+++ b/tests/unit/modules/motion/test_motion.cpp
@@ -205,3 +205,47 @@ TEST_CASE("motion::triple_move", "[motion]") {
     REQUIRE(motion.Position(Selector) == 20);
     REQUIRE(motion.Position(Pulley) == 30);
 }
+
+TEST_CASE("motion::queue_abort", "[motion]") {
+    // queue should start empty
+    REQUIRE(motion.QueueEmpty());
+
+    // enqueue two moves
+    motion.PlanMoveTo(Pulley, 10, 1);
+    motion.PlanMoveTo(Idler, 10, 1);
+    REQUIRE(!motion.QueueEmpty(Pulley));
+    REQUIRE(!motion.QueueEmpty(Idler));
+    REQUIRE(motion.QueueEmpty(Selector));
+    REQUIRE(!motion.QueueEmpty());
+
+    // step ~1/3 way through
+    REQUIRE(stepUntilDone(3) == -1);
+
+    // abort the whole queue
+    motion.AbortPlannedMoves();
+    REQUIRE(motion.QueueEmpty(Pulley));
+    REQUIRE(motion.QueueEmpty(Idler));
+    REQUIRE(motion.QueueEmpty());
+}
+
+TEST_CASE("motion::queue_abort_1", "[motion]") {
+    // queue should start empty
+    REQUIRE(motion.QueueEmpty());
+
+    // enqueue two moves
+    motion.PlanMoveTo(Pulley, 10, 1);
+    motion.PlanMoveTo(Idler, 10, 1);
+    REQUIRE(!motion.QueueEmpty(Pulley));
+    REQUIRE(!motion.QueueEmpty(Idler));
+    REQUIRE(motion.QueueEmpty(Selector));
+    REQUIRE(!motion.QueueEmpty());
+
+    // step ~1/3 way through
+    REQUIRE(stepUntilDone(3) == -1);
+
+    // abort one axis only
+    motion.AbortPlannedMoves(Pulley);
+    REQUIRE(motion.QueueEmpty(Pulley));
+    REQUIRE(!motion.QueueEmpty(Idler));
+    REQUIRE(!motion.QueueEmpty());
+}