Split the stepper ISR routine into multiple inline functions,

added an optimized DDA routine for moves with less than 32767 ticks.

Firmware/planner.cpp

@@ -839,9 +839,7 @@ block->steps_y.wide = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-p
   if(block->steps_x.wide != 0) enable_x();
   if(block->steps_y.wide != 0) enable_y();
   #endif
-#ifndef Z_LATE_ENABLE
   if(block->steps_z.wide != 0) enable_z();
-#endif
 
   // Enable extruder(s)
   if(block->steps_e.wide != 0)
@@ -1234,6 +1232,9 @@ Having the real displacement of the head, we can calculate the total movement le
   block->speed_factor = block->nominal_rate / block->nominal_speed;
   calculate_trapezoid_for_block(block, block->entry_speed, safe_speed);
 
+  if (block->step_event_count.wide <= 32767)
+    block->flag |= BLOCK_FLAG_DDA_LOWRES;
+
   // Move the buffer head. From now the block may be picked up by the stepper interrupt controller.
   block_buffer_head = next_buffer_head;
 

Firmware/planner.h

@@ -50,7 +50,7 @@ union dda_isteps_t
 {
   int32_t     wide;
   struct {
-    uint16_t  lo;
+    int16_t   lo;
     int16_t   hi;
   };
 };
@@ -94,7 +94,7 @@ typedef struct {
   float acceleration;
 
   // Bit flags defined by the BlockFlag enum.
-  bool flag;
+  uint8_t flag;
 
   // Settings for the trapezoid generator (runs inside an interrupt handler).
   // Changing the following values in the planner needs to be synchronized with the interrupt handler by disabling the interrupts.

Firmware/stepper.cpp

@@ -135,8 +135,6 @@ extern bool stepper_timer_overflow_state;
 //=============================functions         ============================
 //===========================================================================
 
-#define CHECK_ENDSTOPS  if(check_endstops)
-
 #ifndef _NO_ASM
 
 // intRes = intIn1 * intIn2 >> 16
@@ -320,7 +318,7 @@ void step_wait(){
 }
 
 
-FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
+FORCE_INLINE unsigned short calc_timer(uint16_t step_rate) {
   unsigned short timer;
   if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
 
@@ -361,10 +359,10 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
 FORCE_INLINE void trapezoid_generator_reset() {
   deceleration_time = 0;
   // step_rate to timer interval
-  OCR1A_nominal = calc_timer(current_block->nominal_rate);
+  OCR1A_nominal = calc_timer(uint16_t(current_block->nominal_rate));
   // make a note of the number of step loops required at nominal speed
   step_loops_nominal = step_loops;
-  acc_step_rate = current_block->initial_rate;
+  acc_step_rate = uint16_t(current_block->initial_rate);
   acceleration_time = calc_timer(acc_step_rate);
   _NEXT_ISR(acceleration_time);
 
@@ -374,7 +372,6 @@ FORCE_INLINE void trapezoid_generator_reset() {
         final_estep_rate = (current_block->nominal_rate * current_block->abs_adv_steps_multiplier8) >> 17;
         }
    #endif
-
 }
 
 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
@@ -391,157 +388,144 @@ ISR(TIMER1_COMPA_vect) {
   #endif
 }
 
-void isr() {
-	//if (UVLO) uvlo();
-  // If there is no current block, attempt to pop one from the buffer
-  if (current_block == NULL) {
-    // Anything in the buffer?
-    current_block = plan_get_current_block();
-    if (current_block != NULL) {
+FORCE_INLINE void stepper_next_block()
+{
+  // Anything in the buffer?
+  current_block = plan_get_current_block();
+  if (current_block != NULL) {
 #ifdef PAT9125
-	  fsensor_counter = 0;
-      fsensor_st_block_begin(current_block);
+    fsensor_counter = 0;
+    fsensor_st_block_begin(current_block);
 #endif //PAT9125
-	  // The busy flag is set by the plan_get_current_block() call.
-      // current_block->busy = true;
-      trapezoid_generator_reset();
+    // The busy flag is set by the plan_get_current_block() call.
+    // current_block->busy = true;
+    trapezoid_generator_reset();
+    if (current_block->flag & BLOCK_FLAG_DDA_LOWRES) {
+      counter_x.lo = -(current_block->step_event_count.lo >> 1);
+      counter_y.lo = counter_x.lo;
+      counter_z.lo = counter_x.lo;
+      counter_e.lo = counter_x.lo;
+    } else {
       counter_x.wide = -(current_block->step_event_count.wide >> 1);
       counter_y.wide = counter_x.wide;
       counter_z.wide = counter_x.wide;
       counter_e.wide = counter_x.wide;
-      step_events_completed.wide = 0;
-
-      #ifdef Z_LATE_ENABLE
-        if(current_block->steps_z.wide > 0) {
-          enable_z();
-          _NEXT_ISR(2000); //1ms wait
-          return;
-        }
-      #endif
     }
-    else {
-        _NEXT_ISR(2000); // 1kHz.
-    }
-  }
-
-	LastStepMask = 0;
-
-  if (current_block != NULL) {
-    // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
+    step_events_completed.wide = 0;
+    // Set directions.
     out_bits = current_block->direction_bits;
-
-
     // Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY)
     if((out_bits & (1<<X_AXIS))!=0){
-        WRITE_NC(X_DIR_PIN, INVERT_X_DIR);
+      WRITE_NC(X_DIR_PIN, INVERT_X_DIR);
       count_direction[X_AXIS]=-1;
-    }
-    else{
-        WRITE_NC(X_DIR_PIN, !INVERT_X_DIR);
+    } else {
+      WRITE_NC(X_DIR_PIN, !INVERT_X_DIR);
       count_direction[X_AXIS]=1;
     }
     if((out_bits & (1<<Y_AXIS))!=0){
       WRITE_NC(Y_DIR_PIN, INVERT_Y_DIR);
-	  
-	  #ifdef Y_DUAL_STEPPER_DRIVERS
-	    WRITE_NC(Y2_DIR_PIN, !(INVERT_Y_DIR == INVERT_Y2_VS_Y_DIR));
-	  #endif
-	  
       count_direction[Y_AXIS]=-1;
-    }
-    else{
+    } else {
       WRITE_NC(Y_DIR_PIN, !INVERT_Y_DIR);
-	  
-	  #ifdef Y_DUAL_STEPPER_DRIVERS
-	    WRITE_NC(Y2_DIR_PIN, (INVERT_Y_DIR == INVERT_Y2_VS_Y_DIR));
-	  #endif
-	  
       count_direction[Y_AXIS]=1;
     }
-
-    // Set direction en check limit switches
-    #ifndef COREXY
-    if ((out_bits & (1<<X_AXIS)) != 0) {   // stepping along -X axis
-    #else
-    if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) != 0)) {   //-X occurs for -A and -B
-    #endif
-      CHECK_ENDSTOPS
-      {
-        {
-          #if ( (defined(X_MIN_PIN) && (X_MIN_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMINLIMIT)
-            
-            #ifdef TMC2130_SG_HOMING
-            // Stall guard homing turned on
-                x_min_endstop = (READ(X_TMC2130_DIAG) != 0);
-            #else
-            // Normal homing
-            x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
-            #endif
-            if(x_min_endstop && old_x_min_endstop && (current_block->steps_x.wide > 0)) {
-              endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
-              endstop_x_hit=true;
-              step_events_completed.wide = current_block->step_event_count.wide;
-            }
-            old_x_min_endstop = x_min_endstop;
-          #endif
-        }
-      }
+    if ((out_bits & (1<<Z_AXIS)) != 0) {   // -direction
+      WRITE_NC(Z_DIR_PIN,INVERT_Z_DIR);
+      count_direction[Z_AXIS]=-1;
+    } else { // +direction
+      WRITE_NC(Z_DIR_PIN,!INVERT_Z_DIR);
+      count_direction[Z_AXIS]=1;
     }
-    else { // +direction
-      CHECK_ENDSTOPS
-      {
-        {
-          #if ( (defined(X_MAX_PIN) && (X_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMAXLIMIT)
-            
-            #ifdef TMC2130_SG_HOMING
-            // Stall guard homing turned on
-                x_max_endstop = (READ(X_TMC2130_DIAG) != 0);
-            #else
-            // Normal homing
-            x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
-            #endif
-            if(x_max_endstop && old_x_max_endstop && (current_block->steps_x.wide > 0)){
-              endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
-              endstop_x_hit=true;
-              step_events_completed.wide = current_block->step_event_count.wide;
-            }
-            old_x_max_endstop = x_max_endstop;
-          #endif
-        }
-      }
+#ifndef LIN_ADVANCE
+    if ((out_bits & (1 << E_AXIS)) != 0) { // -direction
+      WRITE(E0_DIR_PIN, 
+  #ifdef SNMM
+        (snmm_extruder == 0 || snmm_extruder == 2) ? !INVERT_E0_DIR :
+  #endif // SNMM
+        INVERT_E0_DIR);
+      count_direction[E_AXIS] = -1;
+    } else { // +direction
+      WRITE(E0_DIR_PIN,
+  #ifdef SNMM
+        (snmm_extruder == 0 || snmm_extruder == 2) ? INVERT_E0_DIR :
+  #endif // SNMM
+        !INVERT_E0_DIR);
+      count_direction[E_AXIS] = 1;
     }
+#endif /* LIN_ADVANCE */
+  }
+  else {
+      _NEXT_ISR(2000); // 1kHz.
+  }
+}
 
+// Check limit switches.
+FORCE_INLINE void stepper_check_endstops()
+{
+  if(check_endstops) 
+  {
     #ifndef COREXY
-    if ((out_bits & (1<<Y_AXIS)) != 0) {   // -direction
+    if ((out_bits & (1<<X_AXIS)) != 0) // stepping along -X axis
     #else
-    if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) == 0)) {   // -Y occurs for -A and +B
+    if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) != 0)) //-X occurs for -A and -B
     #endif
-      CHECK_ENDSTOPS
-      {
-          
-        #if ( (defined(Y_MIN_PIN) && (Y_MIN_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMINLIMIT)
-                  
+    {
+      #if ( (defined(X_MIN_PIN) && (X_MIN_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMINLIMIT)
+      #ifdef TMC2130_SG_HOMING
+        // Stall guard homing turned on
+        x_min_endstop = (READ(X_TMC2130_DIAG) != 0);
+      #else
+        // Normal homing
+        x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
+      #endif
+        if(x_min_endstop && old_x_min_endstop && (current_block->steps_x.wide > 0)) {
+          endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
+          endstop_x_hit=true;
+          step_events_completed.wide = current_block->step_event_count.wide;
+        }
+        old_x_min_endstop = x_min_endstop;
+      #endif
+    } else { // +direction
+      #if ( (defined(X_MAX_PIN) && (X_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMAXLIMIT)          
         #ifdef TMC2130_SG_HOMING
         // Stall guard homing turned on
-            y_min_endstop = (READ(Y_TMC2130_DIAG) != 0);
+            x_max_endstop = (READ(X_TMC2130_DIAG) != 0);
         #else
         // Normal homing
-        y_min_endstop = (READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
+        x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
         #endif
-          if(y_min_endstop && old_y_min_endstop && (current_block->steps_y.wide > 0)) {
-            endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
-            endstop_y_hit=true;
-            step_events_completed.wide = current_block->step_event_count.wide;
-          }
-          old_y_min_endstop = y_min_endstop;
-        #endif
-      }
+        if(x_max_endstop && old_x_max_endstop && (current_block->steps_x.wide > 0)){
+          endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
+          endstop_x_hit=true;
+          step_events_completed.wide = current_block->step_event_count.wide;
+        }
+        old_x_max_endstop = x_max_endstop;
+      #endif
     }
-    else { // +direction
-      CHECK_ENDSTOPS
-      {
-        #if ( (defined(Y_MAX_PIN) && (Y_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMAXLIMIT)
-                  
+
+    #ifndef COREXY
+    if ((out_bits & (1<<Y_AXIS)) != 0) // -direction
+    #else
+    if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) == 0)) // -Y occurs for -A and +B
+    #endif
+    {        
+      #if ( (defined(Y_MIN_PIN) && (Y_MIN_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMINLIMIT)          
+      #ifdef TMC2130_SG_HOMING
+      // Stall guard homing turned on
+          y_min_endstop = (READ(Y_TMC2130_DIAG) != 0);
+      #else
+      // Normal homing
+      y_min_endstop = (READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
+      #endif
+        if(y_min_endstop && old_y_min_endstop && (current_block->steps_y.wide > 0)) {
+          endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
+          endstop_y_hit=true;
+          step_events_completed.wide = current_block->step_event_count.wide;
+        }
+        old_y_min_endstop = y_min_endstop;
+      #endif
+    } else { // +direction
+      #if ( (defined(Y_MAX_PIN) && (Y_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMAXLIMIT)                
         #ifdef TMC2130_SG_HOMING
         // Stall guard homing turned on
             y_max_endstop = (READ(Y_TMC2130_DIAG) != 0);
@@ -549,195 +533,226 @@ void isr() {
         // Normal homing
         y_max_endstop = (READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
         #endif
-          if(y_max_endstop && old_y_max_endstop && (current_block->steps_y.wide > 0)){
-            endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
-            endstop_y_hit=true;
-            step_events_completed.wide = current_block->step_event_count.wide;
-          }
-          old_y_max_endstop = y_max_endstop;
-        #endif
-      }
-    }
-
-    if ((out_bits & (1<<Z_AXIS)) != 0) {   // -direction
-      WRITE_NC(Z_DIR_PIN,INVERT_Z_DIR);
-      
-      #ifdef Z_DUAL_STEPPER_DRIVERS
-        WRITE_NC(Z2_DIR_PIN,INVERT_Z_DIR);
-      #endif
-
-      count_direction[Z_AXIS]=-1;
-      if(check_endstops && ! check_z_endstop)
-      {
-        #if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT)
-            #ifdef TMC2130_SG_HOMING
-            // Stall guard homing turned on
-                z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0);
-            #else
-				z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
-            #endif //TMC2130_SG_HOMING
-          if(z_min_endstop && old_z_min_endstop && (current_block->steps_z.wide > 0)) {
-            endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-            endstop_z_hit=true;
-            step_events_completed.wide = current_block->step_event_count.wide;
-          }
-          old_z_min_endstop = z_min_endstop;
-        #endif
-      }
-    }
-    else { // +direction
-      WRITE_NC(Z_DIR_PIN,!INVERT_Z_DIR);
-
-      #ifdef Z_DUAL_STEPPER_DRIVERS
-        WRITE_NC(Z2_DIR_PIN,!INVERT_Z_DIR);
+        if(y_max_endstop && old_y_max_endstop && (current_block->steps_y.wide > 0)){
+          endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
+          endstop_y_hit=true;
+          step_events_completed.wide = current_block->step_event_count.wide;
+        }
+        old_y_max_endstop = y_max_endstop;
       #endif
-
-      count_direction[Z_AXIS]=1;
-      CHECK_ENDSTOPS
-      {
-        #if defined(Z_MAX_PIN) && (Z_MAX_PIN > -1) && !defined(DEBUG_DISABLE_ZMAXLIMIT)
-            #ifdef TMC2130_SG_HOMING
-            // Stall guard homing turned on
-                z_max_endstop = (READ(Z_TMC2130_DIAG) != 0);
-            #else
-				z_max_endstop = (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
-            #endif //TMC2130_SG_HOMING
-          if(z_max_endstop && old_z_max_endstop && (current_block->steps_z.wide > 0)) {
-            endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-            endstop_z_hit=true;
-            step_events_completed.wide = current_block->step_event_count.wide;
-          }
-          old_z_max_endstop = z_max_endstop;
-        #endif
-      }
     }
 
-    // Supporting stopping on a trigger of the Z-stop induction sensor, not only for the Z-minus movements.
-    #if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT)
-    if(check_z_endstop) {
-        // Check the Z min end-stop no matter what.
-        // Good for searching for the center of an induction target.
-            #ifdef TMC2130_SG_HOMING
-            // Stall guard homing turned on
-                z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0);
-            #else
-				z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
-            #endif //TMC2130_SG_HOMING
-        if(z_min_endstop && old_z_min_endstop) {
+    if ((out_bits & (1<<Z_AXIS)) != 0) // -direction
+    {
+      #if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT)
+      if (check_z_endstop) {
+        #ifdef TMC2130_SG_HOMING
+          // Stall guard homing turned on
+          z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0);
+        #else
+          z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
+        #endif //TMC2130_SG_HOMING
+        if(z_min_endstop && old_z_min_endstop && (current_block->steps_z.wide > 0)) {
           endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
           endstop_z_hit=true;
           step_events_completed.wide = current_block->step_event_count.wide;
         }
         old_z_min_endstop = z_min_endstop;
+      }
+      #endif
+    } else { // +direction
+      #if defined(Z_MAX_PIN) && (Z_MAX_PIN > -1) && !defined(DEBUG_DISABLE_ZMAXLIMIT)
+        #ifdef TMC2130_SG_HOMING
+        // Stall guard homing turned on
+        z_max_endstop = (READ(Z_TMC2130_DIAG) != 0);
+        #else
+        z_max_endstop = (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
+        #endif //TMC2130_SG_HOMING
+        if(z_max_endstop && old_z_max_endstop && (current_block->steps_z.wide > 0)) {
+          endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+          endstop_z_hit=true;
+          step_events_completed.wide = current_block->step_event_count.wide;
+        }
+        old_z_max_endstop = z_max_endstop;
+      #endif
     }
-    #endif
-
-	if ((out_bits & (1 << E_AXIS)) != 0)
-	{	// -direction
-		//AKU
-    WRITE(E0_DIR_PIN, 
-#ifdef SNMM
-      (snmm_extruder == 0 || snmm_extruder == 2) ? !INVERT_E0_DIR :
-#endif // SNMM
-      INVERT_E0_DIR);
-		count_direction[E_AXIS] = -1;
-	}
-	else
-	{	// +direction
-		WRITE(E0_DIR_PIN,
-#ifdef SNMM
-      (snmm_extruder == 0 || snmm_extruder == 2) ? INVERT_E0_DIR :
-#endif // SNMM
-      !INVERT_E0_DIR);
-		count_direction[E_AXIS] = 1;
-	}
+  }
 
-    for(uint8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
-      #ifndef AT90USB
-      MSerial.checkRx(); // Check for serial chars.
-      #endif //RP - returned, because missing characters
+  // Supporting stopping on a trigger of the Z-stop induction sensor, not only for the Z-minus movements.
+  #if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT)
+  if (check_z_endstop) {
+      // Check the Z min end-stop no matter what.
+      // Good for searching for the center of an induction target.
+      #ifdef TMC2130_SG_HOMING
+      // Stall guard homing turned on
+        z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0);
+      #else
+        z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
+      #endif //TMC2130_SG_HOMING
+      if(z_min_endstop && old_z_min_endstop) {
+        endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+        endstop_z_hit=true;
+        step_events_completed.wide = current_block->step_event_count.wide;
+      }
+      old_z_min_endstop = z_min_endstop;
+  }
+  #endif
+}
 
+FORCE_INLINE void stepper_tick_lowres()
+{
+  for (uint8_t i=0; i < step_loops; ++ i) { // Take multiple steps per interrupt (For high speed moves)
+    MSerial.checkRx(); // Check for serial chars.
 #ifdef LIN_ADVANCE
-        counter_e.wide += current_block->steps_e.wide;
-        if (counter_e.wide > 0) {
-          counter_e.wide -= current_block->step_event_count.wide;
-          count_position[E_AXIS] += count_direction[E_AXIS];
-          ((out_bits&(1<<E_AXIS))!=0) ? --e_steps : ++e_steps;
-        }
+    counter_e.lo += current_block->steps_e.lo;
+    if (counter_e.lo > 0) {
+      counter_e.lo -= current_block->step_event_count.lo;
+      count_position[E_AXIS] += count_direction[E_AXIS];
+      ((out_bits&(1<<E_AXIS))!=0) ? --e_steps : ++e_steps;
+    }
 #endif
-        
-        counter_x.wide += current_block->steps_x.wide;
-        if (counter_x.wide > 0) {
-          WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN);
-		  LastStepMask |= X_AXIS_MASK;
+    // Step in X axis
+    counter_x.lo += current_block->steps_x.lo;
+    if (counter_x.lo > 0) {
+      WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN);
+      LastStepMask |= X_AXIS_MASK;
 #ifdef DEBUG_XSTEP_DUP_PIN
-    WRITE_NC(DEBUG_XSTEP_DUP_PIN,!INVERT_X_STEP_PIN);
+      WRITE_NC(DEBUG_XSTEP_DUP_PIN,!INVERT_X_STEP_PIN);
 #endif //DEBUG_XSTEP_DUP_PIN
-          counter_x.wide -= current_block->step_event_count.wide;
-          count_position[X_AXIS]+=count_direction[X_AXIS];   
-          WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN);
+      counter_x.lo -= current_block->step_event_count.lo;
+      count_position[X_AXIS]+=count_direction[X_AXIS];
+      WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN);
 #ifdef DEBUG_XSTEP_DUP_PIN
-    WRITE_NC(DEBUG_XSTEP_DUP_PIN,INVERT_X_STEP_PIN);
+      WRITE_NC(DEBUG_XSTEP_DUP_PIN,INVERT_X_STEP_PIN);
 #endif //DEBUG_XSTEP_DUP_PIN
-        }
-
-        counter_y.wide += current_block->steps_y.wide;
-        if (counter_y.wide > 0) {
-          WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
-		  LastStepMask |= Y_AXIS_MASK;
+    }
+    // Step in Y axis
+    counter_y.lo += current_block->steps_y.lo;
+    if (counter_y.lo > 0) {
+      WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
+      LastStepMask |= Y_AXIS_MASK;
 #ifdef DEBUG_YSTEP_DUP_PIN
-    WRITE_NC(DEBUG_YSTEP_DUP_PIN,!INVERT_Y_STEP_PIN);
+      WRITE_NC(DEBUG_YSTEP_DUP_PIN,!INVERT_Y_STEP_PIN);
 #endif //DEBUG_YSTEP_DUP_PIN
-		  
-		  #ifdef Y_DUAL_STEPPER_DRIVERS
-			WRITE_NC(Y2_STEP_PIN, !INVERT_Y_STEP_PIN);
-		  #endif
-		  
-          counter_y.wide -= current_block->step_event_count.wide;
-          count_position[Y_AXIS]+=count_direction[Y_AXIS];
-          WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN);
+      counter_y.lo -= current_block->step_event_count.lo;
+      count_position[Y_AXIS]+=count_direction[Y_AXIS];
+      WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN);
 #ifdef DEBUG_YSTEP_DUP_PIN
-    WRITE_NC(DEBUG_YSTEP_DUP_PIN,INVERT_Y_STEP_PIN);
-#endif //DEBUG_YSTEP_DUP_PIN
-		  
-		  #ifdef Y_DUAL_STEPPER_DRIVERS
-			WRITE_NC(Y2_STEP_PIN, INVERT_Y_STEP_PIN);
-		  #endif
-        }
-
-      counter_z.wide += current_block->steps_z.wide;
-      if (counter_z.wide > 0) {
-        WRITE_NC(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
-        LastStepMask |= Z_AXIS_MASK;
-        #ifdef Z_DUAL_STEPPER_DRIVERS
-          WRITE_NC(Z2_STEP_PIN, !INVERT_Z_STEP_PIN);
-        #endif
-
-        counter_z.wide -= current_block->step_event_count.wide;
-        count_position[Z_AXIS]+=count_direction[Z_AXIS];
-        WRITE_NC(Z_STEP_PIN, INVERT_Z_STEP_PIN);
-        
-        #ifdef Z_DUAL_STEPPER_DRIVERS
-          WRITE_NC(Z2_STEP_PIN, INVERT_Z_STEP_PIN);
-        #endif
-      }
-
+      WRITE_NC(DEBUG_YSTEP_DUP_PIN,INVERT_Y_STEP_PIN);
+#endif //DEBUG_YSTEP_DUP_PIN    
+    }
+    // Step in Z axis
+    counter_z.lo += current_block->steps_z.lo;
+    if (counter_z.lo > 0) {
+      WRITE_NC(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
+      LastStepMask |= Z_AXIS_MASK;
+      counter_z.lo -= current_block->step_event_count.lo;
+      count_position[Z_AXIS]+=count_direction[Z_AXIS];
+      WRITE_NC(Z_STEP_PIN, INVERT_Z_STEP_PIN);
+    }
 #ifndef LIN_ADVANCE
-        counter_e.wide += current_block->steps_e.wide;
-        if (counter_e.wide > 0) {
-          WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
-          counter_e.wide -= current_block->step_event_count.wide;
-          count_position[E_AXIS]+=count_direction[E_AXIS];
-          WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
+    // Step in E axis
+    counter_e.lo += current_block->steps_e.lo;
+    if (counter_e.lo > 0) {
+      WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
+      counter_e.lo -= current_block->step_event_count.lo;
+      count_position[E_AXIS]+=count_direction[E_AXIS];
+      WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
 #ifdef PAT9125
-          fsensor_counter++;
+      ++ fsensor_counter;
 #endif //PAT9125
-        }
+    }
 #endif
-        
-      ++ step_events_completed.wide;
-      if(step_events_completed.wide >= current_block->step_event_count.wide) break;
+    if(++ step_events_completed.lo >= current_block->step_event_count.lo)
+      break;
+  }
+}
+
+FORCE_INLINE void stepper_tick_highres()
+{
+  for (uint8_t i=0; i < step_loops; ++ i) { // Take multiple steps per interrupt (For high speed moves)
+    MSerial.checkRx(); // Check for serial chars.
+#ifdef LIN_ADVANCE
+    counter_e.wide += current_block->steps_e.wide;
+    if (counter_e.wide > 0) {
+      counter_e.wide -= current_block->step_event_count.wide;
+      count_position[E_AXIS] += count_direction[E_AXIS];
+      ((out_bits&(1<<E_AXIS))!=0) ? --e_steps : ++e_steps;
+    }
+#endif
+    // Step in X axis
+    counter_x.wide += current_block->steps_x.wide;
+    if (counter_x.wide > 0) {
+      WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN);
+      LastStepMask |= X_AXIS_MASK;
+#ifdef DEBUG_XSTEP_DUP_PIN
+      WRITE_NC(DEBUG_XSTEP_DUP_PIN,!INVERT_X_STEP_PIN);
+#endif //DEBUG_XSTEP_DUP_PIN
+      counter_x.wide -= current_block->step_event_count.wide;
+      count_position[X_AXIS]+=count_direction[X_AXIS];   
+      WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN);
+#ifdef DEBUG_XSTEP_DUP_PIN
+      WRITE_NC(DEBUG_XSTEP_DUP_PIN,INVERT_X_STEP_PIN);
+#endif //DEBUG_XSTEP_DUP_PIN
+    }
+    // Step in Y axis
+    counter_y.wide += current_block->steps_y.wide;
+    if (counter_y.wide > 0) {
+      WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
+      LastStepMask |= Y_AXIS_MASK;
+#ifdef DEBUG_YSTEP_DUP_PIN
+      WRITE_NC(DEBUG_YSTEP_DUP_PIN,!INVERT_Y_STEP_PIN);
+#endif //DEBUG_YSTEP_DUP_PIN
+      counter_y.wide -= current_block->step_event_count.wide;
+      count_position[Y_AXIS]+=count_direction[Y_AXIS];
+      WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN);
+#ifdef DEBUG_YSTEP_DUP_PIN
+      WRITE_NC(DEBUG_YSTEP_DUP_PIN,INVERT_Y_STEP_PIN);
+#endif //DEBUG_YSTEP_DUP_PIN    
+    }
+    // Step in Z axis
+    counter_z.wide += current_block->steps_z.wide;
+    if (counter_z.wide > 0) {
+      WRITE_NC(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
+      LastStepMask |= Z_AXIS_MASK;
+      counter_z.wide -= current_block->step_event_count.wide;
+      count_position[Z_AXIS]+=count_direction[Z_AXIS];
+      WRITE_NC(Z_STEP_PIN, INVERT_Z_STEP_PIN);
+    }
+#ifndef LIN_ADVANCE
+    // Step in E axis
+    counter_e.wide += current_block->steps_e.wide;
+    if (counter_e.wide > 0) {
+      WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
+      counter_e.wide -= current_block->step_event_count.wide;
+      count_position[E_AXIS]+=count_direction[E_AXIS];
+      WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
+#ifdef PAT9125
+      ++ fsensor_counter;
+#endif //PAT9125
     }
+#endif
+    if(++ step_events_completed.wide >= current_block->step_event_count.wide)
+      break;
+  }
+}
+
+void isr() {
+	//if (UVLO) uvlo();
+  // If there is no current block, attempt to pop one from the buffer
+  if (current_block == NULL)
+    stepper_next_block();
+
+	LastStepMask = 0;
+
+  if (current_block != NULL) 
+  {
+    stepper_check_endstops();
+    if (current_block->flag & BLOCK_FLAG_DDA_LOWRES)
+      stepper_tick_lowres();
+    else
+      stepper_tick_highres();
+
 #ifdef LIN_ADVANCE
       if (current_block->use_advance_lead) {
         const int delta_adv_steps = current_estep_rate - current_adv_steps;
@@ -754,10 +769,10 @@ void isr() {
     if (step_events_completed.wide <= (unsigned long int)current_block->accelerate_until) {
       // v = t * a   ->   acc_step_rate = acceleration_time * current_block->acceleration_rate
       MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
-      acc_step_rate += current_block->initial_rate;
+      acc_step_rate += uint16_t(current_block->initial_rate);
 
       // upper limit
-      if(acc_step_rate > current_block->nominal_rate)
+      if(acc_step_rate > uint16_t(current_block->nominal_rate))
         acc_step_rate = current_block->nominal_rate;
 
       // step_rate to timer interval
@@ -776,15 +791,15 @@ void isr() {
       MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
 
       if(step_rate > acc_step_rate) { // Check step_rate stays positive
-        step_rate = current_block->final_rate;
+        step_rate = uint16_t(current_block->final_rate);
       }
       else {
         step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
       }
 
       // lower limit
-      if(step_rate < current_block->final_rate)
-        step_rate = current_block->final_rate;
+      if(step_rate < uint16_t(current_block->final_rate))
+        step_rate = uint16_t(current_block->final_rate);
 
       // step_rate to timer interval
       timer = calc_timer(step_rate);
@@ -830,9 +845,11 @@ void isr() {
 	}
 #endif //PAT9125
   }
+
 #ifdef TMC2130
 	tmc2130_st_isr(LastStepMask);
 #endif //TMC2130
+
 #ifdef DEBUG_STEPPER_TIMER_MISSED
   // Verify whether the next planned timer interrupt has not been missed already.  
   // This debugging test takes < 1.125us