From ad096c9d14f5b7e09cce4554e8b7a31625cc1fc0 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 7 Sep 2021 11:54:54 +0200
Subject: [PATCH] Motion: make Motion::Step finally inline

---
 src/modules/motion.cpp | 33 ---------------------------------
 src/modules/motion.h   | 36 ++++++++++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+), 33 deletions(-)

diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index 67b3d4c..0a4ab40 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -75,39 +75,6 @@ void Motion::AbortPlannedMoves(bool halt) {
         AbortPlannedMoves((Axis)i, halt);
 }
 
-st_timer_t Motion::Step() {
-    st_timer_t timers[NUM_AXIS];
-
-    // step and calculate interval for each new move
-    for (uint8_t i = 0; i != NUM_AXIS; ++i) {
-        timers[i] = axisData[i].residual;
-        if (timers[i] <= config::stepTimerQuantum) {
-            if (timers[i] || !axisData[i].ctrl.QueueEmpty()) {
-                if (st_timer_t next = axisData[i].ctrl.Step(axisParams[i].params)) {
-                    timers[i] += next;
-
-                    // axis has been moved, run the tmc2130 Isr for this axis
-                    axisData[i].drv.Isr(axisParams[i].params);
-                }
-            }
-        }
-    }
-
-    // plan next closest interval
-    st_timer_t next = timers[0];
-    for (uint8_t i = 1; i != NUM_AXIS; ++i) {
-        if (timers[i] && (!next || timers[i] < next))
-            next = timers[i];
-    }
-
-    // update residuals
-    for (uint8_t i = 0; i != NUM_AXIS; ++i) {
-        axisData[i].residual = (timers[i] ? timers[i] - next : 0);
-    }
-
-    return next;
-}
-
 static inline void Isr() {
 #ifdef __AVR__
     st_timer_t next = motion.Step();
diff --git a/src/modules/motion.h b/src/modules/motion.h
index f9c560e..3e44195 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -232,7 +232,43 @@ public:
 
     /// State machine doing all the planning and stepping. Called by the stepping ISR.
     /// @returns the interval for the next tick
+#if !defined(UNITTEST) || defined(UNITTEST_MOTION)
+    inline st_timer_t Step() {
+        st_timer_t timers[NUM_AXIS];
+
+        // step and calculate interval for each new move
+        for (uint8_t i = 0; i != NUM_AXIS; ++i) {
+            timers[i] = axisData[i].residual;
+            if (timers[i] <= config::stepTimerQuantum) {
+                if (timers[i] || !axisData[i].ctrl.QueueEmpty()) {
+                    if (st_timer_t next = axisData[i].ctrl.Step(axisParams[i].params)) {
+                        timers[i] += next;
+
+                        // axis has been moved, run the tmc2130 Isr for this axis
+                        axisData[i].drv.Isr(axisParams[i].params);
+                    }
+                }
+            }
+        }
+
+        // plan next closest interval
+        st_timer_t next = timers[0];
+        for (uint8_t i = 1; i != NUM_AXIS; ++i) {
+            if (timers[i] && (!next || timers[i] < next))
+                next = timers[i];
+        }
+
+        // update residuals
+        for (uint8_t i = 0; i != NUM_AXIS; ++i) {
+            axisData[i].residual = (timers[i] ? timers[i] - next : 0);
+        }
+
+        return next;
+    }
+#else
+    // Force STUB for testing
     st_timer_t Step();
+#endif
 
     /// @returns true if all planned moves have been finished for all axes
     bool QueueEmpty() const;