Unify step pulse timing of ISR / babystep (#16813)

Marlin/src/module/stepper.cpp

@@ -402,6 +402,7 @@ constexpr uint32_t NS_TO_PULSE_TIMER_TICKS(uint32_t NS) { return (NS + (NS_PER_P
 #define PULSE_HIGH_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_HIGH_NS - _MIN(_MIN_PULSE_HIGH_NS, TIMER_SETUP_NS)))
 #define PULSE_LOW_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_LOW_NS - _MIN(_MIN_PULSE_LOW_NS, TIMER_SETUP_NS)))
 
+#define USING_TIMED_PULSE() hal_timer_t end_tick_count = 0
 #define START_TIMED_PULSE(DIR) (end_tick_count = HAL_timer_get_count(PULSE_TIMER_NUM) + PULSE_##DIR##_TICK_COUNT)
 #define AWAIT_TIMED_PULSE() while (HAL_timer_get_count(PULSE_TIMER_NUM) < end_tick_count) { }
 #define START_HIGH_PULSE()  START_TIMED_PULSE(HIGH)
@@ -409,6 +410,18 @@ constexpr uint32_t NS_TO_PULSE_TIMER_TICKS(uint32_t NS) { return (NS + (NS_PER_P
 #define AWAIT_HIGH_PULSE()  AWAIT_TIMED_PULSE()
 #define AWAIT_LOW_PULSE()   AWAIT_TIMED_PULSE()
 
+#if MINIMUM_STEPPER_PRE_DIR_DELAY > 0
+  #define DIR_WAIT_BEFORE() DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY)
+#else
+  #define DIR_WAIT_BEFORE()
+#endif
+
+#if MINIMUM_STEPPER_POST_DIR_DELAY > 0
+  #define DIR_WAIT_AFTER() DELAY_NS(MINIMUM_STEPPER_POST_DIR_DELAY)
+#else
+  #define DIR_WAIT_AFTER()
+#endif
+
 void Stepper::wake_up() {
   // TCNT1 = 0;
   ENABLE_STEPPER_DRIVER_INTERRUPT();
@@ -423,9 +436,7 @@ void Stepper::wake_up() {
  */
 void Stepper::set_directions() {
 
-  #if MINIMUM_STEPPER_PRE_DIR_DELAY > 0
-    DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY);
-  #endif
+  DIR_WAIT_BEFORE();
 
   #define SET_STEP_DIR(A)                       \
     if (motor_direction(_AXIS(A))) {            \
@@ -494,10 +505,7 @@ void Stepper::set_directions() {
     }
   #endif
 
-  // A small delay may be needed after changing direction
-  #if MINIMUM_STEPPER_POST_DIR_DELAY > 0
-    DELAY_NS(MINIMUM_STEPPER_POST_DIR_DELAY);
-  #endif
+  DIR_WAIT_AFTER();
 }
 
 #if ENABLED(S_CURVE_ACCELERATION)
@@ -1488,12 +1496,12 @@ void Stepper::stepper_pulse_phase_isr() {
   // Take multiple steps per interrupt (For high speed moves)
   #if ISR_MULTI_STEPS
     bool firstStep = true;
-    hal_timer_t end_tick_count = 0;
+    USING_TIMED_PULSE();
   #endif
   xyze_bool_t step_needed{0};
 
   do {
-    #define _APPLY_STEP(AXIS) AXIS ##_APPLY_STEP
+    #define _APPLY_STEP(AXIS, INV, ALWAYS) AXIS ##_APPLY_STEP(INV, ALWAYS)
     #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN
 
     // Determine if a pulse is needed using Bresenham
@@ -1509,14 +1517,14 @@ void Stepper::stepper_pulse_phase_isr() {
     // Start an active pulse, if Bresenham says so, and update position
     #define PULSE_START(AXIS) do{ \
       if (step_needed[_AXIS(AXIS)]) { \
-        _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), 0); \
+        _APPLY_STEP(AXIS, !_INVERT_STEP_PIN(AXIS), 0); \
       } \
     }while(0)
 
     // Stop an active pulse, if any, and adjust error term
     #define PULSE_STOP(AXIS) do { \
       if (step_needed[_AXIS(AXIS)]) { \
-        _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS), 0); \
+        _APPLY_STEP(AXIS, _INVERT_STEP_PIN(AXIS), 0); \
       } \
     }while(0)
 
@@ -1978,9 +1986,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
     else
       interval = LA_ADV_NEVER;
 
-    #if MINIMUM_STEPPER_PRE_DIR_DELAY > 0
-      DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY);
-    #endif
+    DIR_WAIT_BEFORE();
 
     #if ENABLED(MIXING_EXTRUDER)
       // We don't know which steppers will be stepped because LA loop follows,
@@ -1996,17 +2002,14 @@ uint32_t Stepper::stepper_block_phase_isr() {
         REV_E_DIR(stepper_extruder);
     #endif
 
-    // A small delay may be needed after changing direction
-    #if MINIMUM_STEPPER_POST_DIR_DELAY > 0
-      DELAY_NS(MINIMUM_STEPPER_POST_DIR_DELAY);
-    #endif
+    DIR_WAIT_AFTER();
 
     //const hal_timer_t added_step_ticks = hal_timer_t(ADDED_STEP_TICKS);
 
     // Step E stepper if we have steps
     #if ISR_MULTI_STEPS
       bool firstStep = true;
-      hal_timer_t end_tick_count = 0;
+      USING_TIMED_PULSE();
     #endif
 
     while (LA_steps) {
@@ -2424,57 +2427,52 @@ void Stepper::report_positions() {
 
 #if ENABLED(BABYSTEPPING)
 
-  #if MINIMUM_STEPPER_PULSE
-    #define STEP_PULSE_CYCLES ((MINIMUM_STEPPER_PULSE) * CYCLES_PER_MICROSECOND)
-  #else
-    #define STEP_PULSE_CYCLES 0
-  #endif
-
-  #if ENABLED(DELTA)
-    #define CYCLES_EATEN_BABYSTEP (2 * 15)
-  #else
-    #define CYCLES_EATEN_BABYSTEP 0
-  #endif
-  #define EXTRA_CYCLES_BABYSTEP (STEP_PULSE_CYCLES - (CYCLES_EATEN_BABYSTEP))
-
   #define _ENABLE_AXIS(AXIS) ENABLE_AXIS_## AXIS()
   #define _READ_DIR(AXIS) AXIS ##_DIR_READ()
   #define _INVERT_DIR(AXIS) INVERT_## AXIS ##_DIR
   #define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true)
 
-  #if EXTRA_CYCLES_BABYSTEP > 20
-    #define _SAVE_START const hal_timer_t pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM)
-    #define _PULSE_WAIT while (EXTRA_CYCLES_BABYSTEP > (uint32_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
-  #else
-    #define _SAVE_START NOOP
-    #if EXTRA_CYCLES_BABYSTEP > 0
-      #define _PULSE_WAIT DELAY_NS(EXTRA_CYCLES_BABYSTEP * NANOSECONDS_PER_CYCLE)
-    #elif ENABLED(DELTA)
-      #define _PULSE_WAIT DELAY_US(2);
-    #elif STEP_PULSE_CYCLES > 0
-      #define _PULSE_WAIT NOOP
-    #else
-      #define _PULSE_WAIT DELAY_US(4);
-    #endif
-  #endif
-
-  #define BABYSTEP_AXIS(AXIS, INVERT, DIR) {            \
+  #if DISABLED(DELTA)
+    #define BABYSTEP_AXIS(AXIS, INV, DIR) do{           \
       const uint8_t old_dir = _READ_DIR(AXIS);          \
       _ENABLE_AXIS(AXIS);                               \
-      DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY);          \
-      _APPLY_DIR(AXIS, _INVERT_DIR(AXIS)^DIR^INVERT);   \
-      DELAY_NS(MINIMUM_STEPPER_POST_DIR_DELAY);         \
-      _SAVE_START;                                      \
-      _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), true); \
-      _PULSE_WAIT;                                      \
-      _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS), true);  \
+      DIR_WAIT_BEFORE();                                \
+      _APPLY_DIR(AXIS, _INVERT_DIR(AXIS)^DIR^INV);      \
+      DIR_WAIT_AFTER();                                 \
+      USING_TIMED_PULSE();                              \
+      START_HIGH_PULSE();                               \
+      _APPLY_STEP(AXIS, !_INVERT_STEP_PIN(AXIS), true); \
+      AWAIT_HIGH_PULSE();                               \
+      _APPLY_STEP(AXIS,  _INVERT_STEP_PIN(AXIS), true); \
       _APPLY_DIR(AXIS, old_dir);                        \
-    }
+    }while(0)
+  #endif
+
+  #if IS_CORE
+    #define BABYSTEP_CORE(A, B, INV, DIR) do{     \
+      const xy_byte_t old_dir = { _READ_DIR(A), _READ_DIR(B) }; \
+      _ENABLE_AXIS(A); _ENABLE_AXIS(B);           \
+      DIR_WAIT_BEFORE();                          \
+      _APPLY_DIR(A, _INVERT_DIR(A)^DIR^INV);      \
+      _APPLY_DIR(B, _INVERT_DIR(B)^DIR^INV^(CORESIGN(1)<0)); \
+      DIR_WAIT_AFTER();                           \
+      USING_TIMED_PULSE();                        \
+      START_HIGH_PULSE();                         \
+      _APPLY_STEP(A, !_INVERT_STEP_PIN(A), true); \
+      _APPLY_STEP(B, !_INVERT_STEP_PIN(B), true); \
+      AWAIT_HIGH_PULSE();                         \
+      _APPLY_STEP(A,  _INVERT_STEP_PIN(A), true); \
+      _APPLY_STEP(B,  _INVERT_STEP_PIN(B), true); \
+      _APPLY_DIR(A, old_dir.a); _APPLY_DIR(B, old_dir.b); \
+    }while(0)
+  #endif
 
   // MUST ONLY BE CALLED BY AN ISR,
   // No other ISR should ever interrupt this!
   void Stepper::babystep(const AxisEnum axis, const bool direction) {
-    cli();
+    DISABLE_ISRS();
+
+    USING_TIMED_PULSE();
 
     switch (axis) {
 
@@ -2482,11 +2480,9 @@ void Stepper::report_positions() {
 
         case X_AXIS:
           #if CORE_IS_XY
-            BABYSTEP_AXIS(X, false, direction);
-            BABYSTEP_AXIS(Y, false, direction);
+            BABYSTEP_CORE(X, Y, false, direction);
           #elif CORE_IS_XZ
-            BABYSTEP_AXIS(X, false, direction);
-            BABYSTEP_AXIS(Z, false, direction);
+            BABYSTEP_CORE(X, Z, false, direction);
           #else
             BABYSTEP_AXIS(X, false, direction);
           #endif
@@ -2494,11 +2490,9 @@ void Stepper::report_positions() {
 
         case Y_AXIS:
           #if CORE_IS_XY
-            BABYSTEP_AXIS(X, false, direction);
-            BABYSTEP_AXIS(Y, false, direction^(CORESIGN(1)<0));
+            BABYSTEP_CORE(X, Y, false, direction);
           #elif CORE_IS_YZ
-            BABYSTEP_AXIS(Y, false, direction);
-            BABYSTEP_AXIS(Z, false, direction^(CORESIGN(1)<0));
+            BABYSTEP_CORE(Y, Z, false, direction);
           #else
             BABYSTEP_AXIS(Y, false, direction);
           #endif
@@ -2509,13 +2503,9 @@ void Stepper::report_positions() {
       case Z_AXIS: {
 
         #if CORE_IS_XZ
-          BABYSTEP_AXIS(X, BABYSTEP_INVERT_Z, direction);
-          BABYSTEP_AXIS(Z, BABYSTEP_INVERT_Z, direction^(CORESIGN(1)<0));
-
+          BABYSTEP_CORE(X, Z, BABYSTEP_INVERT_Z, direction);
         #elif CORE_IS_YZ
-          BABYSTEP_AXIS(Y, BABYSTEP_INVERT_Z, direction);
-          BABYSTEP_AXIS(Z, BABYSTEP_INVERT_Z, direction^(CORESIGN(1)<0));
-
+          BABYSTEP_CORE(Y, Z, BABYSTEP_INVERT_Z, direction);
         #elif DISABLED(DELTA)
           BABYSTEP_AXIS(Z, BABYSTEP_INVERT_Z, direction);
 
@@ -2527,38 +2517,32 @@ void Stepper::report_positions() {
           ENABLE_AXIS_Y();
           ENABLE_AXIS_Z();
 
-          #if MINIMUM_STEPPER_PRE_DIR_DELAY > 0
-            DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY);
-          #endif
+          DIR_WAIT_BEFORE();
 
-          const uint8_t old_x_dir_pin = X_DIR_READ(),
-                        old_y_dir_pin = Y_DIR_READ(),
-                        old_z_dir_pin = Z_DIR_READ();
+          const xyz_byte_t old_dir = { X_DIR_READ(), Y_DIR_READ(), Z_DIR_READ() };
 
           X_DIR_WRITE(INVERT_X_DIR ^ z_direction);
           Y_DIR_WRITE(INVERT_Y_DIR ^ z_direction);
           Z_DIR_WRITE(INVERT_Z_DIR ^ z_direction);
 
-          #if MINIMUM_STEPPER_POST_DIR_DELAY > 0
-            DELAY_NS(MINIMUM_STEPPER_POST_DIR_DELAY);
-          #endif
+          DIR_WAIT_AFTER();
 
-          _SAVE_START;
+          START_HIGH_PULSE();
 
           X_STEP_WRITE(!INVERT_X_STEP_PIN);
           Y_STEP_WRITE(!INVERT_Y_STEP_PIN);
           Z_STEP_WRITE(!INVERT_Z_STEP_PIN);
 
-          _PULSE_WAIT;
+          AWAIT_HIGH_PULSE();
 
           X_STEP_WRITE(INVERT_X_STEP_PIN);
           Y_STEP_WRITE(INVERT_Y_STEP_PIN);
           Z_STEP_WRITE(INVERT_Z_STEP_PIN);
 
           // Restore direction bits
-          X_DIR_WRITE(old_x_dir_pin);
-          Y_DIR_WRITE(old_y_dir_pin);
-          Z_DIR_WRITE(old_z_dir_pin);
+          X_DIR_WRITE(old_dir.x);
+          Y_DIR_WRITE(old_dir.y);
+          Z_DIR_WRITE(old_dir.z);
 
         #endif
 
@@ -2566,7 +2550,9 @@ void Stepper::report_positions() {
 
       default: break;
     }
-    sei();
+
+    START_LOW_PULSE(); AWAIT_LOW_PULSE();  // Prevent Stepper::ISR pulsing too soon
+    ENABLE_ISRS();                         // Now it's ok for the ISR to run
   }
 
 #endif // BABYSTEPPING