Merge pull request #10615 from thinkyhead/bf1_synced_planner_set_position

[1.1.x] Improve sync of planner / stepper position, asynchronous G92

Marlin/G26_Mesh_Validation_Tool.cpp

@@ -204,8 +204,6 @@
       destination[E_AXIS] = current_position[E_AXIS];
 
       G26_line_to_destination(feed_value);
-
-      stepper.synchronize();
       set_destination_from_current();
     }
 
@@ -220,8 +218,6 @@
     destination[E_AXIS] += e_delta;
 
     G26_line_to_destination(feed_value);
-
-    stepper.synchronize();
     set_destination_from_current();
   }
 
@@ -267,13 +263,12 @@
             if (Total_Prime >= EXTRUDE_MAXLENGTH) return G26_ERR;
           #endif
           G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0);
-
+          set_destination_from_current();
           stepper.synchronize();    // Without this synchronize, the purge is more consistent,
                                     // but because the planner has a buffer, we won't be able
                                     // to stop as quickly. So we put up with the less smooth
                                     // action to give the user a more responsive 'Stop'.
-          set_destination_from_current();
+
-          idle();
           SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
         }
 
@@ -295,7 +290,6 @@
       set_destination_from_current();
       destination[E_AXIS] += g26_prime_length;
       G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0);
-      stepper.synchronize();
       set_destination_from_current();
       retract_filament(destination);
     }
@@ -703,7 +697,6 @@
 
     if (current_position[Z_AXIS] < Z_CLEARANCE_BETWEEN_PROBES) {
       do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
-      stepper.synchronize();
       set_current_from_destination();
     }
 
@@ -738,7 +731,7 @@
       lcd_external_control = true;
     #endif
 
-//  debug_current_and_destination(PSTR("Starting G26 Mesh Validation Pattern."));
+    //debug_current_and_destination(PSTR("Starting G26 Mesh Validation Pattern."));
 
     /**
      * Pre-generate radius offset values at 30 degree intervals to reduce CPU load.

Marlin/I2CPositionEncoder.cpp

@@ -358,7 +358,7 @@
 
     stepper.synchronize();
 
-    planner.buffer_line(startCoord[X_AXIS],startCoord[Y_AXIS],startCoord[Z_AXIS],
+    planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS],
                         stepper.get_axis_position_mm(E_AXIS), feedrate, 0);
     stepper.synchronize();
 
@@ -415,10 +415,10 @@
     startCoord[encoderAxis] = startDistance;
     endCoord[encoderAxis] = endDistance;
 
-    LOOP_L_N(i, iter) {
     stepper.synchronize();
 
-      planner.buffer_line(startCoord[X_AXIS],startCoord[Y_AXIS],startCoord[Z_AXIS],
+    LOOP_L_N(i, iter) {
+      planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS],
                           stepper.get_axis_position_mm(E_AXIS), feedrate, 0);
       stepper.synchronize();
 
@@ -427,7 +427,7 @@
 
       //do_blocking_move_to(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS]);
 
-      planner.buffer_line(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS],
+      planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS],
                           stepper.get_axis_position_mm(E_AXIS), feedrate, 0);
       stepper.synchronize();
 

Marlin/Marlin_main.cpp

@@ -2984,7 +2984,7 @@ static void do_homing_move(const AxisEnum axis, const float distance, const floa
     planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
   #else
     sync_plan_position();
-    current_position[axis] = distance;
+    current_position[axis] = distance; // Set delta/cartesian axes directly
     planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
   #endif
 
@@ -5337,8 +5337,8 @@ void home_all_axes() { gcode_G28(true); }
         #if ENABLED(DEBUG_LEVELING_FEATURE)
           if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
         #endif
-        enqueue_and_echo_commands_P(PSTR(Z_PROBE_END_SCRIPT));
         stepper.synchronize();
+        enqueue_and_echo_commands_P(PSTR(Z_PROBE_END_SCRIPT));
       #endif
 
       // Auto Bed Leveling is complete! Enable if possible.
@@ -6134,9 +6134,8 @@ void home_all_axes() { gcode_G28(true); }
       }
     #endif
 
-    stepper.synchronize();  // wait until the machine is idle
-
     // Move until destination reached or target hit
+    stepper.synchronize();
     endstops.enable(true);
     G38_move = true;
     G38_endstop_hit = false;
@@ -6158,13 +6157,13 @@ void home_all_axes() { gcode_G28(true); }
         LOOP_XYZ(i) destination[i] += retract_mm[i];
         endstops.enable(false);
         prepare_move_to_destination();
-        stepper.synchronize();
 
         feedrate_mm_s /= 4;
 
         // Bump the target more slowly
         LOOP_XYZ(i) destination[i] -= retract_mm[i] * 2;
 
+        stepper.synchronize();
         endstops.enable(true);
         G38_move = true;
         prepare_move_to_destination();
@@ -6258,8 +6257,6 @@ void home_all_axes() { gcode_G28(true); }
  */
 inline void gcode_G92() {
 
-  stepper.synchronize();
-
   #if ENABLED(CNC_COORDINATE_SYSTEMS)
     switch (parser.subcode) {
       case 1:
@@ -6319,10 +6316,9 @@ inline void gcode_G92() {
       COPY(coordinate_system[active_coordinate_system], position_shift);
   #endif
 
-  if (didXYZ)
+  // Update planner/steppers only if the native coordinates changed
-    SYNC_PLAN_POSITION_KINEMATIC();
+  if    (didXYZ) SYNC_PLAN_POSITION_KINEMATIC();
-  else if (didE)
+  else if (didE) sync_plan_position_e();
-    sync_plan_position_e();
 
   report_current_position();
 }
@@ -6349,6 +6345,8 @@ inline void gcode_G92() {
 
     const bool has_message = !hasP && !hasS && args && *args;
 
+    stepper.synchronize();
+
     #if ENABLED(ULTIPANEL)
 
       if (has_message)
@@ -6372,8 +6370,6 @@ inline void gcode_G92() {
     KEEPALIVE_STATE(PAUSED_FOR_USER);
     wait_for_user = true;
 
-    stepper.synchronize();
-
     if (ms > 0) {
       ms += millis();  // wait until this time for a click
       while (PENDING(millis(), ms) && wait_for_user) idle();
@@ -6524,8 +6520,8 @@ inline void gcode_M17() {
     set_destination_from_current();
     destination[E_AXIS] += length / planner.e_factor[active_extruder];
     planner.buffer_line_kinematic(destination, fr, active_extruder);
-    stepper.synchronize();
     set_current_from_destination();
+    stepper.synchronize();
   }
 
   static float resume_position[XYZE];
@@ -6813,12 +6809,12 @@ inline void gcode_M17() {
     #endif
     print_job_timer.pause();
 
-    // Wait for synchronize steppers
-    stepper.synchronize();
-
     // Save current position
     COPY(resume_position, current_position);
 
+    // Wait for synchronize steppers
+    stepper.synchronize();
+
     // Initial retract before move to filament change position
     if (retract && thermalManager.hotEnoughToExtrude(active_extruder))
       do_pause_e_move(retract, PAUSE_PARK_RETRACT_FEEDRATE);
@@ -8508,7 +8504,6 @@ inline void gcode_M81() {
   safe_delay(1000); // Wait 1 second before switching off
 
   #if HAS_SUICIDE
-    stepper.synchronize();
     suicide();
   #elif HAS_POWER_SWITCH
     PSU_OFF();
@@ -8644,8 +8639,6 @@ void report_current_position() {
 
   void report_current_position_detail() {
 
-    stepper.synchronize();
-
     SERIAL_PROTOCOLPGM("\nLogical:");
     const float logical[XYZ] = {
       LOGICAL_X_POSITION(current_position[X_AXIS]),
@@ -8680,6 +8673,8 @@ void report_current_position() {
       report_xyz(delta);
     #endif
 
+    stepper.synchronize();
+
     SERIAL_PROTOCOLPGM("Stepper:");
     LOOP_XYZE(i) {
       SERIAL_CHAR(' ');
@@ -13369,8 +13364,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
               current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS],
               planner.max_feedrate_mm_s[X_AXIS], 1
             );
-            SYNC_PLAN_POSITION_KINEMATIC();
             stepper.synchronize();
+            SYNC_PLAN_POSITION_KINEMATIC();
             extruder_duplication_enabled = true;
             active_extruder_parked = false;
             #if ENABLED(DEBUG_LEVELING_FEATURE)
@@ -13978,6 +13973,7 @@ void manage_inactivity(const bool ignore_stepper_queue/*=false*/) {
       planner.buffer_line_kinematic(current_position, MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder);
       current_position[E_AXIS] = olde;
       planner.set_e_position_mm(olde);
+
       stepper.synchronize();
       #if ENABLED(SWITCHING_EXTRUDER)
         E0_ENABLE_WRITE(oldstatus);

Marlin/fwretract.cpp

@@ -107,7 +107,7 @@ void FWRetract::retract(const bool retracting
     // G11 priority to recover the long retract if activated
     if (!retracting) swapping = retracted_swap[active_extruder];
   #else
-    const bool swapping = false;
+    constexpr bool swapping = false;
   #endif
 
   /* // debugging
@@ -127,54 +127,47 @@ void FWRetract::retract(const bool retracting
     SERIAL_ECHOLNPAIR("hop_amount ", hop_amount);
   //*/
 
-  const float old_feedrate_mm_s = feedrate_mm_s;
+  const float old_feedrate_mm_s = feedrate_mm_s,
+              renormalize = RECIPROCAL(planner.e_factor[active_extruder]),
+              base_retract = swapping ? swap_retract_length : retract_length,
+              old_z = current_position[Z_AXIS],
+              old_e = current_position[E_AXIS];
 
   // The current position will be the destination for E and Z moves
   set_destination_from_current();
-  stepper.synchronize();  // Wait for buffered moves to complete
-
-  const float renormalize = 1.0 / planner.e_factor[active_extruder];
 
   if (retracting) {
     // Retract by moving from a faux E position back to the current E position
     feedrate_mm_s = retract_feedrate_mm_s;
-    current_position[E_AXIS] += (swapping ? swap_retract_length : retract_length) * renormalize;
+    destination[E_AXIS] -= base_retract * renormalize;
-    sync_plan_position_e();
     prepare_move_to_destination();                        // set_current_to_destination
 
     // Is a Z hop set, and has the hop not yet been done?
-    // No double zlifting
-    // Feedrate to the max
     if (retract_zlift > 0.01 && !hop_amount) {            // Apply hop only once
-      const float old_z = current_position[Z_AXIS];
       hop_amount += retract_zlift;                        // Add to the hop total (again, only once)
       destination[Z_AXIS] += retract_zlift;               // Raise Z by the zlift (M207 Z) amount
       feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS];  // Maximum Z feedrate
       prepare_move_to_destination();                      // Raise up, set_current_to_destination
-      current_position[Z_AXIS] = old_z;                   // Spoof the Z position in the planner
-      SYNC_PLAN_POSITION_KINEMATIC();
     }
   }
   else {
     // If a hop was done and Z hasn't changed, undo the Z hop
     if (hop_amount) {
-      current_position[Z_AXIS] += hop_amount;             // Set actual Z (due to the prior hop)
+      destination[Z_AXIS] -= hop_amount;                  // Move back down by the total hop amount
-      SYNC_PLAN_POSITION_KINEMATIC();                     // Spoof the Z position in the planner
       feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS];  // Z feedrate to max
       prepare_move_to_destination();                      // Lower Z, set_current_to_destination
       hop_amount = 0.0;                                   // Clear the hop amount
     }
 
-    // A retract multiplier has been added here to get faster swap recovery
+    destination[E_AXIS] += (base_retract + (swapping ? swap_retract_recover_length : retract_recover_length)) * renormalize;
     feedrate_mm_s = swapping ? swap_retract_recover_feedrate_mm_s : retract_recover_feedrate_mm_s;
-
-    current_position[E_AXIS] -= (swapping ? swap_retract_length + swap_retract_recover_length
-                                          : retract_length + retract_recover_length) * renormalize;
-    sync_plan_position_e();
     prepare_move_to_destination();                        // Recover E, set_current_to_destination
   }
 
   feedrate_mm_s = old_feedrate_mm_s;                      // Restore original feedrate
+  current_position[Z_AXIS] = old_z;                       // Restore Z and E positions
+  current_position[E_AXIS] = old_e;
+  SYNC_PLAN_POSITION_KINEMATIC();                         // As if the move never took place
 
   retracted[active_extruder] = retracting;                // Active extruder now retracted / recovered
 

Marlin/planner.cpp

@@ -819,15 +819,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
   const float esteps_float = de * e_factor[extruder];
   const int32_t esteps = abs(esteps_float) + 0.5;
 
-  // Calculate the buffer head after we push this byte
+  // Wait for the next available block
-  const uint8_t next_buffer_head = next_block_index(block_buffer_head);
+  uint8_t next_buffer_head;
-
+  block_t * const block = get_next_free_block(next_buffer_head);
-  // If the buffer is full: good! That means we are well ahead of the robot.
-  // Rest here until there is room in the buffer.
-  while (block_buffer_tail == next_buffer_head) idle();
-
-  // Prepare to set up new block
-  block_t* block = &block_buffer[block_buffer_head];
 
   // Clear all flags, including the "busy" bit
   block->flag = 0x00;
@@ -1467,6 +1461,26 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
 
 } // _buffer_steps()
 
+/**
+ * Planner::buffer_sync_block
+ * Add a block to the buffer that just updates the position
+ */
+void Planner::buffer_sync_block() {
+  // Wait for the next available block
+  uint8_t next_buffer_head;
+  block_t * const block = get_next_free_block(next_buffer_head);
+
+  block->steps[A_AXIS] = position[A_AXIS];
+  block->steps[B_AXIS] = position[B_AXIS];
+  block->steps[C_AXIS] = position[C_AXIS];
+  block->steps[E_AXIS] = position[E_AXIS];
+
+  block->flag = BLOCK_FLAG_SYNC_POSITION;
+
+  block_buffer_head = next_buffer_head;
+  stepper.wake_up();
+} // buffer_sync_block()
+
 /**
  * Planner::buffer_segment
  *
@@ -1595,19 +1609,19 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
   #else
     #define _EINDEX E_AXIS
   #endif
-  const int32_t na = position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]),
+  position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]),
-                nb = position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]),
+  position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]),
-                nc = position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]),
+  position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]),
-                ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
+  position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
   #if HAS_POSITION_FLOAT
-    position_float[X_AXIS] = a;
+    position_float[A_AXIS] = a;
-    position_float[Y_AXIS] = b;
+    position_float[B_AXIS] = b;
-    position_float[Z_AXIS] = c;
+    position_float[C_AXIS] = c;
     position_float[E_AXIS] = e;
   #endif
-  stepper.set_position(na, nb, nc, ne);
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
   ZERO(previous_speed);
+  buffer_sync_block();
 }
 
 void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
@@ -1655,23 +1669,23 @@ void Planner::set_position_mm(const AxisEnum axis, const float &v) {
   #if HAS_POSITION_FLOAT
     position_float[axis] = v;
   #endif
-  stepper.set_position(axis, position[axis]);
   previous_speed[axis] = 0.0;
+  buffer_sync_block();
 }
 
 // Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
 void Planner::reset_acceleration_rates() {
   #if ENABLED(DISTINCT_E_FACTORS)
-    #define HIGHEST_CONDITION (i < E_AXIS || i == E_AXIS + active_extruder)
+    #define AXIS_CONDITION (i < E_AXIS || i == E_AXIS + active_extruder)
   #else
-    #define HIGHEST_CONDITION true
+    #define AXIS_CONDITION true
   #endif
   uint32_t highest_rate = 1;
   LOOP_XYZE_N(i) {
     max_acceleration_steps_per_s2[i] = max_acceleration_mm_per_s2[i] * axis_steps_per_mm[i];
-    if (HIGHEST_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]);
+    if (AXIS_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]);
   }
-  cutoff_long = 4294967295UL / highest_rate;
+  cutoff_long = 4294967295UL / highest_rate; // 0xFFFFFFFFUL
 }
 
 // Recalculate position, steps_to_mm if axis_steps_per_mm changes!

Marlin/planner.h

@@ -53,14 +53,18 @@ enum BlockFlagBit : char {
   BLOCK_BIT_BUSY,
 
   // The block is segment 2+ of a longer move
-  BLOCK_BIT_CONTINUED
+  BLOCK_BIT_CONTINUED,
+
+  // Sync the stepper counts from the block
+  BLOCK_BIT_SYNC_POSITION
 };
 
 enum BlockFlag : char {
   BLOCK_FLAG_RECALCULATE          = _BV(BLOCK_BIT_RECALCULATE),
   BLOCK_FLAG_NOMINAL_LENGTH       = _BV(BLOCK_BIT_NOMINAL_LENGTH),
   BLOCK_FLAG_BUSY                 = _BV(BLOCK_BIT_BUSY),
-  BLOCK_FLAG_CONTINUED            = _BV(BLOCK_BIT_CONTINUED)
+  BLOCK_FLAG_CONTINUED            = _BV(BLOCK_BIT_CONTINUED),
+  BLOCK_FLAG_SYNC_POSITION        = _BV(BLOCK_BIT_SYNC_POSITION)
 };
 
 /**
@@ -409,6 +413,20 @@ class Planner {
 
     #endif
 
+
+    /**
+     * Planner::get_next_free_block
+     *
+     * - Get the next head index (passed by reference)
+     * - Wait for a space to open up in the planner
+     * - Return the head block
+     */
+    FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head) {
+      next_buffer_head = next_block_index(block_buffer_head);
+      while (block_buffer_tail == next_buffer_head) idle(); // while (is_full)
+      return &block_buffer[block_buffer_head];
+    }
+
     /**
      * Planner::_buffer_steps
      *
@@ -426,6 +444,12 @@ class Planner {
       , float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
     );
 
+    /**
+     * Planner::buffer_sync_block
+     * Add a block to the buffer that just updates the position
+     */
+    static void buffer_sync_block();
+
     /**
      * Planner::buffer_segment
      *
@@ -505,7 +529,7 @@ class Planner {
     static void set_position_mm_kinematic(const float (&cart)[XYZE]);
     static void set_position_mm(const AxisEnum axis, const float &v);
     FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
-    FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(AxisEnum(E_AXIS), e); }
+    FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); }
 
     /**
      * Sync from the stepper positions. (e.g., after an interrupted move)
@@ -515,7 +539,7 @@ class Planner {
     /**
      * Does the buffer have any blocks queued?
      */
-    static inline bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
+    FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
 
     /**
      * "Discard" the block and "release" the memory.

Marlin/stepper.cpp

@@ -91,7 +91,7 @@ int16_t Stepper::cleaning_buffer_counter = 0;
   bool Stepper::locked_z_motor = false, Stepper::locked_z2_motor = false;
 #endif
 
-long Stepper::counter_X = 0,
+int32_t Stepper::counter_X = 0,
         Stepper::counter_Y = 0,
         Stepper::counter_Z = 0,
         Stepper::counter_E = 0;
@@ -123,13 +123,13 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
 
 #endif // LIN_ADVANCE
 
-long Stepper::acceleration_time, Stepper::deceleration_time;
+int32_t Stepper::acceleration_time, Stepper::deceleration_time;
 
-volatile long Stepper::count_position[NUM_AXIS] = { 0 };
+volatile int32_t Stepper::count_position[NUM_AXIS] = { 0 };
 volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
 
 #if ENABLED(MIXING_EXTRUDER)
-  long Stepper::counter_m[MIXING_STEPPERS];
+  int32_t Stepper::counter_m[MIXING_STEPPERS];
 #endif
 
 uint8_t Stepper::step_loops, Stepper::step_loops_nominal;
@@ -137,7 +137,7 @@ uint8_t Stepper::step_loops, Stepper::step_loops_nominal;
 uint16_t Stepper::OCR1A_nominal,
          Stepper::acc_step_rate; // needed for deceleration start point
 
-volatile long Stepper::endstops_trigsteps[XYZ];
+volatile int32_t Stepper::endstops_trigsteps[XYZ];
 
 #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
   #define LOCKED_X_MOTOR  locked_x_motor
@@ -449,18 +449,30 @@ void Stepper::isr() {
 
   // If there is no current block, attempt to pop one from the buffer
   if (!current_block) {
+
     // Anything in the buffer?
     if ((current_block = planner.get_current_block())) {
+
+      // Sync block? Sync the stepper counts and return
+      while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
+        _set_position(
+          current_block->steps[A_AXIS], current_block->steps[B_AXIS],
+          current_block->steps[C_AXIS], current_block->steps[E_AXIS]
+        );
+        planner.discard_current_block();
+        if (!(current_block = planner.get_current_block())) return;
+      }
+
       trapezoid_generator_reset();
 
       // Initialize Bresenham counters to 1/2 the ceiling
       counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1);
-
       #if ENABLED(MIXING_EXTRUDER)
         MIXING_STEPPERS_LOOP(i)
           counter_m[i] = -(current_block->mix_event_count[i] >> 1);
       #endif
 
+      // No step events completed so far
       step_events_completed = 0;
 
       #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
@@ -469,6 +481,7 @@ void Stepper::isr() {
       #endif
 
       #if ENABLED(Z_LATE_ENABLE)
+        // If delayed Z enable, postpone move for 1mS
         if (current_block->steps[Z_AXIS] > 0) {
           enable_Z();
           _NEXT_ISR(2000); // Run at slow speed - 1 KHz
@@ -595,7 +608,6 @@ void Stepper::isr() {
     #endif
 
     #if ENABLED(LIN_ADVANCE)
-
       counter_E += current_block->steps[E_AXIS];
       if (counter_E > 0) {
         #if DISABLED(MIXING_EXTRUDER)
@@ -708,7 +720,6 @@ void Stepper::isr() {
     acceleration_time += interval;
 
     #if ENABLED(LIN_ADVANCE)
-
       if (current_block->use_advance_lead) {
         if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
           nextAdvanceISR = 0; // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached
@@ -719,7 +730,6 @@ void Stepper::isr() {
         eISR_Rate = ADV_NEVER;
         if (e_steps) nextAdvanceISR = 0;
       }
-
     #endif // LIN_ADVANCE
   }
   else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
@@ -742,7 +752,6 @@ void Stepper::isr() {
     deceleration_time += interval;
 
     #if ENABLED(LIN_ADVANCE)
-
       if (current_block->use_advance_lead) {
         if (step_events_completed <= (uint32_t)current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
           nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
@@ -753,16 +762,13 @@ void Stepper::isr() {
         eISR_Rate = ADV_NEVER;
         if (e_steps) nextAdvanceISR = 0;
       }
-
     #endif // LIN_ADVANCE
   }
   else {
 
     #if ENABLED(LIN_ADVANCE)
-
       // If we have esteps to execute, fire the next advance_isr "now"
       if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0;
-
     #endif
 
     SPLIT(OCR1A_nominal);  // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
@@ -902,6 +908,7 @@ void Stepper::isr() {
   }
 
   void Stepper::advance_isr_scheduler() {
+
     // Run main stepping ISR if flagged
     if (!nextMainISR) isr();
 
@@ -1120,12 +1127,7 @@ void Stepper::synchronize() { while (planner.has_blocks_queued() || cleaning_buf
  * This allows get_axis_position_mm to correctly
  * derive the current XYZ position later on.
  */
-void Stepper::set_position(const long &a, const long &b, const long &c, const long &e) {
+void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
-
-  synchronize(); // Bad to set stepper counts in the middle of a move
-
-  CRITICAL_SECTION_START;
-
   #if CORE_IS_XY
     // corexy positioning
     // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
@@ -1148,29 +1150,15 @@ void Stepper::set_position(const long &a, const long &b, const long &c, const lo
     count_position[Y_AXIS] = b;
     count_position[Z_AXIS] = c;
   #endif
-
   count_position[E_AXIS] = e;
-  CRITICAL_SECTION_END;
-}
-
-void Stepper::set_position(const AxisEnum &axis, const long &v) {
-  CRITICAL_SECTION_START;
-  count_position[axis] = v;
-  CRITICAL_SECTION_END;
-}
-
-void Stepper::set_e_position(const long &e) {
-  CRITICAL_SECTION_START;
-  count_position[E_AXIS] = e;
-  CRITICAL_SECTION_END;
 }
 
 /**
  * Get a stepper's position in steps.
  */
-long Stepper::position(const AxisEnum axis) {
+int32_t Stepper::position(const AxisEnum axis) {
   CRITICAL_SECTION_START;
-  const long count_pos = count_position[axis];
+  const int32_t count_pos = count_position[axis];
   CRITICAL_SECTION_END;
   return count_pos;
 }
@@ -1239,7 +1227,7 @@ void Stepper::endstop_triggered(const AxisEnum axis) {
 
 void Stepper::report_positions() {
   CRITICAL_SECTION_START;
-  const long xpos = count_position[X_AXIS],
+  const int32_t xpos = count_position[X_AXIS],
                 ypos = count_position[Y_AXIS],
                 zpos = count_position[Z_AXIS];
   CRITICAL_SECTION_END;

Marlin/stepper.h

@@ -119,7 +119,7 @@ class Stepper {
     #endif
 
     // Counter variables for the Bresenham line tracer
-    static long counter_X, counter_Y, counter_Z, counter_E;
+    static int32_t counter_X, counter_Y, counter_Z, counter_E;
     static volatile uint32_t step_events_completed; // The number of step events executed in the current block
 
     #if ENABLED(LIN_ADVANCE)
@@ -142,19 +142,19 @@ class Stepper {
 
     #endif // !LIN_ADVANCE
 
-    static long acceleration_time, deceleration_time;
+    static int32_t acceleration_time, deceleration_time;
     static uint8_t step_loops, step_loops_nominal;
 
     static uint16_t OCR1A_nominal,
                     acc_step_rate; // needed for deceleration start point
 
-    static volatile long endstops_trigsteps[XYZ];
+    static volatile int32_t endstops_trigsteps[XYZ];
-    static volatile long endstops_stepsTotal, endstops_stepsDone;
+    static volatile int32_t endstops_stepsTotal, endstops_stepsDone;
 
     //
     // Positions of stepper motors, in step units
     //
-    static volatile long count_position[NUM_AXIS];
+    static volatile int32_t count_position[NUM_AXIS];
 
     //
     // Current direction of stepper motors (+1 or -1)
@@ -165,7 +165,7 @@ class Stepper {
     // Mixing extruder mix counters
     //
     #if ENABLED(MIXING_EXTRUDER)
-      static long counter_m[MIXING_STEPPERS];
+      static int32_t counter_m[MIXING_STEPPERS];
       #define MIXING_STEPPERS_LOOP(VAR) \
         for (uint8_t VAR = 0; VAR < MIXING_STEPPERS; VAR++) \
           if (current_block->mix_event_count[VAR])
@@ -202,9 +202,32 @@ class Stepper {
     //
     // Set the current position in steps
     //
-    static void set_position(const long &a, const long &b, const long &c, const long &e);
+    static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
-    static void set_position(const AxisEnum &a, const long &v);
+
-    static void set_e_position(const long &e);
+    FORCE_INLINE static void _set_position(const AxisEnum a, const int32_t &v) { count_position[a] = v; }
+
+    FORCE_INLINE static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
+      synchronize();
+      CRITICAL_SECTION_START;
+      _set_position(a, b, c, e);
+      CRITICAL_SECTION_END;
+    }
+
+    static void set_position(const AxisEnum a, const int32_t &v) {
+      synchronize();
+      CRITICAL_SECTION_START;
+      count_position[a] = v;
+      CRITICAL_SECTION_END;
+    }
+
+    FORCE_INLINE static void _set_e_position(const int32_t &e) { count_position[E_AXIS] = e; }
+
+    static void set_e_position(const int32_t &e) {
+      synchronize();
+      CRITICAL_SECTION_START;
+      count_position[E_AXIS] = e;
+      CRITICAL_SECTION_END;
+    }
 
     //
     // Set direction bits for all steppers
@@ -214,7 +237,7 @@ class Stepper {
     //
     // Get the position of a stepper, in steps
     //
-    static long position(const AxisEnum axis);
+    static int32_t position(const AxisEnum axis);
 
     //
     // Report the positions of the steppers, in steps