From f1efce7e525a651f0fcd08005e310ff7f21e6c8b Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 23 Jun 2020 16:40:39 +0200
Subject: [PATCH] Handle LA termination with double/quad stepping properly

Before PR #2591 LA was automatically capped during cruising or
deceleration. However we now rely on reaching the current pressure state
exactly to stop. When dual/quad stepping inside the eISR we might incur
in oscillating behavior if we do not handle it correctly.

This might be the cause behind #2757

This now changes e_step_loops to be a phase-local variable, so we now
reset it each phase too (instead of per-segment).
---
 Firmware/stepper.cpp | 21 ++++++++++++++-------
 1 file changed, 14 insertions(+), 7 deletions(-)

diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index 0c7ecae5..13724cc9 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -348,10 +348,7 @@ FORCE_INLINE void stepper_next_block()
 
 #ifdef LIN_ADVANCE
     if (current_block->use_advance_lead) {
-        e_step_loops = current_block->advance_step_loops;
         target_adv_steps = current_block->max_adv_steps;
-    } else {
-        e_step_loops = 1;
     }
     e_steps = 0;
     nextAdvanceISR = ADV_NEVER;
@@ -871,7 +868,10 @@ FORCE_INLINE void isr() {
             nextAdvanceISR = ADV_NEVER;
         }
         else {
+            // reset error and iterations per loop for this phase
             eISR_Err = current_block->advance_rate / 4;
+            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
                 LA_phase = 0;
@@ -929,15 +929,22 @@ FORCE_INLINE void isr() {
 FORCE_INLINE void advance_isr() {
     if (current_adv_steps > target_adv_steps) {
         // decompression
+        if (e_step_loops != 1) {
+            uint16_t d_steps = current_adv_steps - target_adv_steps;
+            if (d_steps < e_step_loops)
+                e_step_loops = d_steps;
+        }
         e_steps -= e_step_loops;
         if (e_steps) WRITE_NC(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
-        if(current_adv_steps > e_step_loops)
-            current_adv_steps -= e_step_loops;
-        else
-            current_adv_steps = 0;
+        current_adv_steps -= e_step_loops;
     }
     else if (current_adv_steps < target_adv_steps) {
         // compression
+        if (e_step_loops != 1) {
+            uint16_t d_steps = target_adv_steps - current_adv_steps;
+            if (d_steps < e_step_loops)
+                e_step_loops = d_steps;
+        }
         e_steps += e_step_loops;
         if (e_steps) WRITE_NC(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
         current_adv_steps += e_step_loops;