From aad9c0ed8d6cb61c701aae4cd94da8d9619c4dd9 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 8 May 2016 12:16:26 -0700
Subject: [PATCH] Apply updated ISR timing code

---
 Marlin/stepper.cpp | 34 ++++++++++++++++++++--------------
 Marlin/stepper.h   |  2 +-
 2 files changed, 21 insertions(+), 15 deletions(-)

diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index bc070178e3..c9d49cd12c 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -413,6 +413,12 @@ void Stepper::isr() {
       step_events_completed++;
       if (step_events_completed >= current_block->step_event_count) break;
     }
+
+    #if ENABLED(LIN_ADVANCE)
+      // If we have esteps to execute, fire the next ISR "now"
+      if (e_steps[current_block->active_extruder]) OCR0A = TCNT0 + 2;
+    #endif
+
     // Calculate new timer value
     unsigned short timer, step_rate;
     if (step_events_completed <= (unsigned long)current_block->accelerate_until) {
@@ -427,7 +433,7 @@ void Stepper::isr() {
       timer = calc_timer(acc_step_rate);
       OCR1A = timer;
       acceleration_time += timer;
-      
+
       #if ENABLED(LIN_ADVANCE)
 
         if (current_block->use_advance_lead)
@@ -444,6 +450,9 @@ void Stepper::isr() {
 
       #endif // ADVANCE or LIN_ADVANCE
 
+      #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
+        eISR_Rate = (timer >> 2) / abs(e_steps[current_block->active_extruder]);
+      #endif
     }
     else if (step_events_completed > (unsigned long)current_block->decelerate_after) {
       MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
@@ -476,11 +485,20 @@ void Stepper::isr() {
         old_advance = advance_whole;
 
       #endif // ADVANCE or LIN_ADVANCE
+
+      #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
+        eISR_Rate = (timer >> 2) / abs(e_steps[current_block->active_extruder]);
+      #endif
     }
     else {
+
       #if ENABLED(LIN_ADVANCE)
+
         if (current_block->use_advance_lead)
           current_estep_rate[current_block->active_extruder] = final_estep_rate;
+
+        eISR_Rate = (OCR1A_nominal >> 2) / abs(e_steps[current_block->active_extruder]);
+
       #endif
 
       OCR1A = OCR1A_nominal;
@@ -506,19 +524,7 @@ void Stepper::isr() {
 
   void Stepper::advance_isr() {
 
-    byte maxesteps = 0;
-    for (uint8_t i = 0; i < EXTRUDERS; i++)
-      if (abs(e_steps[i]) > maxesteps) maxesteps = abs(e_steps[i]);
-
-    if (maxesteps > 3)
-      old_OCR0A += 13;  // ~19kHz (250000/13 = 19230 Hz)
-    else if (maxesteps > 2)
-      old_OCR0A += 17;  // ~15kHz (250000/17 = 14705 Hz)
-    else if (maxesteps > 1)
-      old_OCR0A += 26;  // ~10kHz (250000/26 =  9615 Hz)
-    else
-      old_OCR0A += 52;  //  ~5kHz (250000/26 =  4807 Hz)
-
+    old_OCR0A += eISR_Rate;
     OCR0A = old_OCR0A;
 
     #define STEP_E_ONCE(INDEX) \
diff --git a/Marlin/stepper.h b/Marlin/stepper.h
index 684ec21bc2..279db1807f 100644
--- a/Marlin/stepper.h
+++ b/Marlin/stepper.h
@@ -22,7 +22,7 @@
 
 /**
  * stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors
- * Part of Grbl
+ * Derived from Grbl
  *
  * Copyright (c) 2009-2011 Simen Svale Skogsrud
  *