From feafc5e5abdc1acee8683411c83c4595885332f5 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Mon, 27 Jul 2020 19:12:56 +0200
Subject: [PATCH] Alternative schedule for LA ticks

Remove most of the original complexity from advance_spread.

Instead of accumulating time to be scheduled, plan ahead of time each
eISR tick using the next main interval + an accumulator (eISR_Err),
which keeps everything much simpler.

The distribution of the advance ticks is now using the real LA
frequency, which leaves a bit more time between the last LA tick and
the main stepper isr.

We take advantage of the accumulator to force a LA tick right after the
first main tick, which removes a +/- 1 scheduling error at higher step
rates.

When decompressing, we force 2 steps instead, so that the direction
reversal happens immediately (first tick zeros esteps, second inverts
the sign), removing another +/- 1 error at higher step rates.
---
 Firmware/stepper.cpp | 39 ++++++++++++++++-----------------------
 1 file changed, 16 insertions(+), 23 deletions(-)

diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index 949ffd58..8b21ee67 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -125,7 +125,7 @@ volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
 
   static uint16_t main_Rate;
   static uint16_t eISR_Rate;
-  static uint16_t eISR_Err;
+  static uint32_t eISR_Err;
 
   static uint16_t current_adv_steps;
   static uint16_t target_adv_steps;
@@ -733,38 +733,30 @@ FORCE_INLINE uint16_t fastdiv(uint16_t q, uint8_t d)
 
 FORCE_INLINE void advance_spread(uint16_t timer)
 {
-    if(eISR_Err > timer)
+    eISR_Err += timer;
+
+    uint8_t ticks = 0;
+    while(eISR_Err >= current_block->advance_rate)
+    {
+        ++ticks;
+        eISR_Err -= current_block->advance_rate;
+    }
+    if(!ticks)
     {
-        // advance-step skipped
-        eISR_Err -= timer;
         eISR_Rate = timer;
         nextAdvanceISR = timer;
         return;
     }
 
-    // at least one step
-    uint8_t ticks = 1;
-    uint32_t block = current_block->advance_rate;
-    uint16_t max_t = timer - eISR_Err;
-    while (block < max_t)
-    {
-        ++ticks;
-        block += current_block->advance_rate;
-    }
-    if (block > timer)
-        eISR_Err += block - timer;
-    else
-        eISR_Err -= timer - block;
-
-    if (ticks <= 4)
-        eISR_Rate = fastdiv(timer, ticks);
+    if (ticks <= 3)
+        eISR_Rate = fastdiv(timer, ticks + 1);
     else
     {
         // >4 ticks are still possible on slow moves
-        eISR_Rate = timer / ticks;
+        eISR_Rate = timer / (ticks + 1);
     }
 
-    nextAdvanceISR = eISR_Rate / 2;
+    nextAdvanceISR = eISR_Rate;
 }
 #endif
 
@@ -876,11 +868,12 @@ FORCE_INLINE void isr() {
         }
         else {
             // reset error and iterations per loop for this phase
-            eISR_Err = current_block->advance_rate / 4;
+            eISR_Err = current_block->advance_rate;
             e_step_loops = current_block->advance_step_loops;
 
             if ((la_state & ADV_ACC_VARY) && e_extruding && (current_adv_steps > target_adv_steps)) {
                 // LA could reverse the direction of extrusion in this phase
+                eISR_Err += current_block->advance_rate;
                 LA_phase = 0;
             }
         }