From 8df5955da46d94539eab890a599aaa75b502ee9b Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Sun, 17 Dec 2017 02:37:35 -0600
Subject: [PATCH] Revert "Reset timer count before first block step"

This reverts commit 5cf6a062e315e7c0936f60ba6bf11d7f8fb1f20c.
---
 Marlin/src/module/stepper.cpp | 21 ++++++---------------
 Marlin/src/module/stepper.h   |  3 +++
 2 files changed, 9 insertions(+), 15 deletions(-)

diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp
index 2d78fa6b0..4372db6b2 100644
--- a/Marlin/src/module/stepper.cpp
+++ b/Marlin/src/module/stepper.cpp
@@ -408,13 +408,10 @@ void Stepper::isr() {
   }
 
   // If there is no current block, attempt to pop one from the buffer
-  bool first_step = false;
   if (!current_block) {
     // Anything in the buffer?
     if ((current_block = planner.get_current_block())) {
       trapezoid_generator_reset();
-      HAL_timer_set_current_count(STEP_TIMER_NUM, 0);
-      first_step = true;
 
       // Initialize Bresenham counters to 1/2 the ceiling
       counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1);
@@ -669,18 +666,12 @@ void Stepper::isr() {
   // Calculate new timer value
   if (step_events_completed <= (uint32_t)current_block->accelerate_until) {
 
-    if (first_step) {
-      acc_step_rate = current_block->initial_rate;
-      acceleration_time = 0;
-    }
-    else {
-      #ifdef CPU_32_BIT
-        MultiU32X24toH32(acc_step_rate, acceleration_time, current_block->acceleration_rate);
-      #else
-        MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
-      #endif
-      acc_step_rate += current_block->initial_rate;
-    }
+    #ifdef CPU_32_BIT
+      MultiU32X24toH32(acc_step_rate, acceleration_time, current_block->acceleration_rate);
+    #else
+      MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
+    #endif
+    acc_step_rate += current_block->initial_rate;
 
     // upper limit
     NOMORE(acc_step_rate, current_block->nominal_rate);
diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h
index 8b27524d8..77e180a2a 100644
--- a/Marlin/src/module/stepper.h
+++ b/Marlin/src/module/stepper.h
@@ -362,6 +362,9 @@ class Stepper {
       OCR1A_nominal = calc_timer_interval(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;
+      acceleration_time = calc_timer_interval(acc_step_rate);
+      _NEXT_ISR(acceleration_time);
 
       #if ENABLED(LIN_ADVANCE)
         if (current_block->use_advance_lead) {