[Bugfix-1.1.x] initial step correction (#8735)

* tested

* forgot one item

Marlin/stepper.cpp

@@ -447,10 +447,13 @@ 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();
+      TCNT1 = 0;  // make sure first pulse is not truncated
+      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);
@@ -705,8 +708,14 @@ 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 {
       MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
       acc_step_rate += current_block->initial_rate;
+    }
 
     // upper limit
     NOMORE(acc_step_rate, current_block->nominal_rate);

Marlin/stepper.h

@@ -359,9 +359,6 @@ 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) {