🩹 Clock-based planner trapezoidal nominal_rate (#26881)

MarlinFirmware · Jul 13, 2024 · 228179e · 228179e
1 parent c961f3a
commit 228179e
Show file tree

Hide file tree

Showing 5 changed files with 18 additions and 17 deletions.
diff --git a/Marlin/src/HAL/ESP32/timers.h b/Marlin/src/HAL/ESP32/timers.h
@@ -53,12 +53,11 @@ typedef uint64_t hal_timer_t;
 #if ENABLED(I2S_STEPPER_STREAM)
   #define STEPPER_TIMER_PRESCALE     1
   #define STEPPER_TIMER_RATE         250000                           // 250khz, 4µs pulses of i2s word clock
-  #define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs // wrong would be 0.25
 #else
   #define STEPPER_TIMER_PRESCALE     40
   #define STEPPER_TIMER_RATE         ((HAL_TIMER_RATE) / (STEPPER_TIMER_PRESCALE)) // frequency of stepper timer, 2MHz
-  #define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000)              // stepper timer ticks per µs
 #endif
+#define STEPPER_TIMER_TICKS_PER_US   ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs
 
 #define STEP_TIMER_MIN_INTERVAL   8 // minimum time in µs between stepper interrupts
 

diff --git a/Marlin/src/HAL/TEENSY31_32/timers.h b/Marlin/src/HAL/TEENSY31_32/timers.h
@@ -41,7 +41,7 @@ typedef uint32_t hal_timer_t;
 #define FTM0_TIMER_PRESCALE_BITS 0b011
 #define FTM1_TIMER_PRESCALE_BITS 0b010
 
-#define FTM0_TIMER_RATE (F_BUS / (FTM0_TIMER_PRESCALE)) // 60MHz / 8 = 7500kHz
+#define FTM0_TIMER_RATE (F_BUS / (FTM0_TIMER_PRESCALE)) // 60MHz / 8 = 7.5MHz
 #define FTM1_TIMER_RATE (F_BUS / (FTM1_TIMER_PRESCALE)) // 60MHz / 4 = 15MHz
 
 #define HAL_TIMER_RATE         (FTM0_TIMER_RATE)

diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp
@@ -729,8 +729,6 @@ void Planner::init() {
   #endif
 #endif
 
-#define MINIMAL_STEP_RATE 120
-
 /**
  * Get the current block for processing
  * and mark the block as busy.
@@ -796,13 +794,9 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
   uint32_t initial_rate = entry_speed ? LROUND(entry_speed * spmm) : block->initial_rate,
            final_rate = LROUND(exit_speed * spmm);
 
-  // Removing code to constrain values produces judder in direction-switching moves because the
-  // current discrete stepping math diverges from physical motion under constant acceleration
-  // when acceleration_steps_per_s2 is large compared to initial/final_rate.
-  NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE));
-  NOLESS(final_rate,   uint32_t(MINIMAL_STEP_RATE));
-  NOMORE(initial_rate, block->nominal_rate);  // NOTE: The nominal rate may be less than MINIMAL_STEP_RATE!
-  NOMORE(final_rate,   block->nominal_rate);
+  NOLESS(initial_rate,        stepper.minimal_step_rate);
+  NOLESS(final_rate,          stepper.minimal_step_rate);
+  NOLESS(block->nominal_rate, stepper.minimal_step_rate);
 
   #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE)
     // If we have some plateau time, the cruise rate will be the nominal rate

diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp
@@ -2201,12 +2201,10 @@ hal_timer_t Stepper::calc_timer_interval(uint32_t step_rate) {
   #ifdef CPU_32_BIT
 
     // A fast processor can just do integer division
-    constexpr uint32_t min_step_rate = uint32_t(STEPPER_TIMER_RATE) / HAL_TIMER_TYPE_MAX;
-    return step_rate > min_step_rate ? uint32_t(STEPPER_TIMER_RATE) / step_rate : HAL_TIMER_TYPE_MAX;
+    return step_rate > minimal_step_rate ? uint32_t(STEPPER_TIMER_RATE) / step_rate : HAL_TIMER_TYPE_MAX;
 
   #else
 
-    constexpr uint32_t min_step_rate = (F_CPU) / 500000U; // i.e., 32 or 40
     if (step_rate >= 0x0800) {  // higher step rate
       // AVR is able to keep up at around 65kHz Stepping ISR rate at most.
       // So values for step_rate > 65535 might as well be truncated.
@@ -2220,8 +2218,8 @@ hal_timer_t Stepper::calc_timer_interval(uint32_t step_rate) {
       const uint8_t gain = uint8_t(pgm_read_byte(table_address + 2));
       return base - MultiU8X8toH8(uint8_t(step_rate & 0x00FF), gain);
     }
-    else if (step_rate > min_step_rate) { // lower step rates
-      step_rate -= min_step_rate; // Correct for minimal speed
+    else if (step_rate > minimal_step_rate) { // lower step rates
+      step_rate -= minimal_step_rate; // Correct for minimal speed
       const uintptr_t table_address = uintptr_t(&speed_lookuptable_slow[uint8_t(step_rate >> 3)]);
       return uint16_t(pgm_read_word(table_address))
              - ((uint16_t(pgm_read_word(table_address + 2)) * uint8_t(step_rate & 0x0007)) >> 3);

diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h
@@ -295,6 +295,16 @@ class Stepper {
 
   public:
 
+    // The minimal step rate ensures calculations stay within limits
+    // and avoid the most unreasonably slow step rates.
+    static constexpr uint32_t minimal_step_rate = (
+      #ifdef CPU_32_BIT
+        _MAX((STEPPER_TIMER_RATE) / HAL_TIMER_TYPE_MAX, 1U) // 32-bit shouldn't go below 1
+      #else
+        (F_CPU) / 500000U   // AVR shouldn't go below 32 (16MHz) or 40 (20MHz)
+      #endif
+    );
+
     #if ANY(HAS_EXTRA_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
       static bool separate_multi_axis;
     #endif