From 9c3b31756eb74906deeefe6896d34b0f6e4a667d Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Sat, 28 Aug 2021 18:13:33 +0200
Subject: [PATCH] Optimize timer multiplexing and increase stepTimerQuantum

Avoid calling PulseGen::Step() on idle axes by checking for a non-zero
queue size (which is more efficient to compute).

Increase stepTimerQuantum to 128us to ensure acceleration can be
computed in realtime for 3 axes at the same time.

Fix the logic of the static assertion, which was flipped: we need to
create slices larger than the maximal step frequency in order to ensure
no axis is starved while moving.
---
 src/config/config.h    |  7 ++++---
 src/modules/motion.cpp | 10 +++++++---
 src/modules/motion.h   |  4 ++--
 3 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/src/config/config.h b/src/config/config.h
index 87a71bf..e7e9c79 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -68,9 +68,10 @@ static constexpr uint8_t blockBufferSize = 2;
 /// Step timer frequency divider (F = F_CPU / divider)
 static constexpr uint8_t stepTimerFrequencyDivider = 8;
 
-/// Smallest stepping ISR scheduling slice (T = F_CPU / divider * quantum)
-/// 16 = 8us (25us is the max frequency interval per maxStepFrequency)
-static constexpr uint8_t stepTimerQuantum = 16;
+/// Smallest stepping ISR scheduling slice (T = 1 / (F_CPU / divider) * quantum)
+/// 25us is the max frequency interval per maxStepFrequency attainable for a single axis
+/// while accelerating: with 3 axes this yields a required minimum of 75us
+static constexpr uint16_t stepTimerQuantum = 256; // 256 = 128us
 
 /// Pulley axis configuration
 static constexpr AxisConfig pulley = {
diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index 2ee45fa..77649fa 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -74,10 +74,14 @@ st_timer_t Motion::Step() {
     for (uint8_t i = 0; i != NUM_AXIS; ++i) {
         timers[i] = axisData[i].residual;
         if (timers[i] <= config::stepTimerQuantum) {
-            timers[i] += axisData[i].ctrl.Step(axisParams[i].params);
+            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);
+                    // axis has been moved, run the tmc2130 Isr for this axis
+                    axisData[i].drv.Isr(axisParams[i].params);
+                }
+            }
         }
     }
 
diff --git a/src/modules/motion.h b/src/modules/motion.h
index 5fd3e08..b2c3c5d 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -18,8 +18,8 @@ using pulse_gen::st_timer_t;
 // Check for configuration invariants
 static_assert(
     (1. / (F_CPU / config::stepTimerFrequencyDivider) * config::stepTimerQuantum)
-        < (1. / config::maxStepFrequency / 2),
-    "stepTimerQuantum must be smaller than the maximal stepping frequency interval");
+        > (1. / config::maxStepFrequency),
+    "stepTimerQuantum must be larger than the maximal stepping frequency interval");
 
 /// Main axis enumeration
 struct AxisParams {