Merge pull request #2785 from wavexx/la15_acc_fixes

Linear Advance 1.5 Fixes

Firmware/Configuration_adv.h

@@ -288,6 +288,7 @@
   #define LA_K_DEF    0        // Default K factor (Unit: mm compression per 1mm/s extruder speed)
   #define LA_K_MAX    10       // Maximum acceptable K factor (exclusive, see notes in planner.cpp:plan_buffer_line)
   #define LA_LA10_MIN LA_K_MAX // Lin. Advance 1.0 threshold value (inclusive)
+  //#define LA_FLOWADJ         // Adjust LA along with flow/M221 for uniform width
   //#define LA_NOCOMPAT        // Disable Linear Advance 1.0 compatibility
   //#define LA_LIVE_K          // Allow adjusting K in the Tune menu
   //#define LA_DEBUG           // If enabled, this will generate debug information output over USB.

Firmware/Marlin.h

@@ -299,7 +299,7 @@ extern float feedrate;
 extern int feedmultiply;
 extern int extrudemultiply; // Sets extrude multiply factor (in percent) for all extruders
 extern int extruder_multiply[EXTRUDERS]; // sets extrude multiply factor (in percent) for each extruder individually
-extern float volumetric_multiplier[EXTRUDERS]; // reciprocal of cross-sectional area of filament (in square millimeters), stored this way to reduce computational burden in planner
+extern float extruder_multiplier[EXTRUDERS]; // reciprocal of cross-sectional area of filament (in square millimeters), stored this way to reduce computational burden in planner
 extern float current_position[NUM_AXIS] ;
 extern float destination[NUM_AXIS] ;
 extern float min_pos[3];

Firmware/planner.cpp

@@ -226,11 +226,23 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
   // Size of Plateau of Nominal Rate.
   uint32_t plateau_steps     = 0;
 
+#ifdef LIN_ADVANCE
+  uint16_t final_adv_steps = 0;
+  uint16_t max_adv_steps = 0;
+  if (block->use_advance_lead) {
+      final_adv_steps = final_rate * block->adv_comp;
+  }
+#endif
+
   // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
   // have to use intersection_distance() to calculate when to abort acceleration and start braking
   // in order to reach the final_rate exactly at the end of this block.
   if (accel_decel_steps < block->step_event_count.wide) {
     plateau_steps = block->step_event_count.wide - accel_decel_steps;
+#ifdef LIN_ADVANCE
+    if (block->use_advance_lead)
+        max_adv_steps = block->nominal_rate * block->adv_comp;
+#endif
   } else {
     uint32_t acceleration_x4  = acceleration << 2;
     // Avoid negative numbers
@@ -263,14 +275,20 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
             decelerate_steps = block->step_event_count.wide;
         accelerate_steps = block->step_event_count.wide - decelerate_steps;
     }
-  }
 
 #ifdef LIN_ADVANCE
-  uint16_t final_adv_steps = 0;
-  if (block->use_advance_lead) {
-      final_adv_steps = exit_speed * block->adv_comp;
-  }
+    if (block->use_advance_lead) {
+        if(!accelerate_steps || !decelerate_steps) {
+            // accelerate_steps=0: deceleration-only ramp, max_rate is effectively unused
+            // decelerate_steps=0: acceleration-only ramp, max_rate _is_ final_rate
+            max_adv_steps = final_adv_steps;
+        } else {
+            float max_rate = sqrt(acceleration_x2 * accelerate_steps + initial_rate_sqr);
+            max_adv_steps = max_rate * block->adv_comp;
+        }
+    }
 #endif
+  }
 
   CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section
   // This block locks the interrupts globally for 4.38 us,
@@ -284,6 +302,7 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
     block->final_rate = final_rate;
 #ifdef LIN_ADVANCE
     block->final_adv_steps = final_adv_steps;
+    block->max_adv_steps = max_adv_steps;
 #endif
   }
   CRITICAL_SECTION_END;
@@ -1077,12 +1096,20 @@ Having the real displacement of the head, we can calculate the total movement le
                               && delta_mm[E_AXIS] >= 0
                               && abs(delta_mm[Z_AXIS]) < 0.5;
     if (block->use_advance_lead) {
+#ifdef LA_FLOWADJ
+        // M221/FLOW should change uniformly the extrusion thickness
+        float delta_e = (e - position_float[E_AXIS]) / extruder_multiplier[extruder];
+#else
+        // M221/FLOW only adjusts for an incorrect source diameter
+        float delta_e = (e - position_float[E_AXIS]);
+#endif
+        float delta_D = sqrt(sq(x - position_float[X_AXIS])
+                             + sq(y - position_float[Y_AXIS])
+                             + sq(z - position_float[Z_AXIS]));
+
         // all extrusion moves with LA require a compression which is proportional to the
         // extrusion_length to distance ratio (e/D)
-        e_D_ratio = (e - position_float[E_AXIS]) /
-                    sqrt(sq(x - position_float[X_AXIS])
-                         + sq(y - position_float[Y_AXIS])
-                         + sq(z - position_float[Z_AXIS]));
+        e_D_ratio = delta_e / delta_D;
 
         // Check for unusual high e_D ratio to detect if a retract move was combined with the last
         // print move due to min. steps per segment. Never execute this with advance! This assumes
@@ -1134,52 +1161,6 @@ Having the real displacement of the head, we can calculate the total movement le
 
   block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));
 
-#ifdef LIN_ADVANCE
-  if (block->use_advance_lead) {
-      // the nominal speed doesn't change past this point: calculate the compression ratio for the
-      // segment and the required advance steps
-      block->adv_comp = extruder_advance_K * e_D_ratio * cs.axis_steps_per_unit[E_AXIS];
-      block->max_adv_steps = block->nominal_speed * block->adv_comp;
-
-      float advance_speed;
-      if (e_D_ratio > 0)
-          advance_speed = (extruder_advance_K * e_D_ratio * block->acceleration * cs.axis_steps_per_unit[E_AXIS]);
-      else
-          advance_speed = cs.max_jerk[E_AXIS] * cs.axis_steps_per_unit[E_AXIS];
-
-      // to save more space we avoid another copy of calc_timer and go through slow division, but we
-      // still need to replicate the *exact* same step grouping policy (see below)
-      if (advance_speed > MAX_STEP_FREQUENCY) advance_speed = MAX_STEP_FREQUENCY;
-      float advance_rate = (F_CPU / 8.0) / advance_speed;
-      if (advance_speed > 20000) {
-          block->advance_rate = advance_rate * 4;
-          block->advance_step_loops = 4;
-      }
-      else if (advance_speed > 10000) {
-          block->advance_rate = advance_rate * 2;
-          block->advance_step_loops = 2;
-      }
-      else
-      {
-          // never overflow the internal accumulator with very low rates
-          if (advance_rate < UINT16_MAX)
-              block->advance_rate = advance_rate;
-          else
-              block->advance_rate = UINT16_MAX;
-          block->advance_step_loops = 1;
-      }
-
-      #ifdef LA_DEBUG
-      if (block->advance_step_loops > 2)
-          // @wavexx: we should really check for the difference between step_loops and
-          //          advance_step_loops instead. A difference of more than 1 will lead
-          //          to uneven speed and *should* be adjusted here by furthermore
-          //          reducing the speed.
-          SERIAL_ECHOLNPGM("LA: More than 2 steps per eISR loop executed.");
-      #endif
-  }
-#endif
-
   // Start with a safe speed.
   // Safe speed is the speed, from which the machine may halt to stop immediately.
   float safe_speed = block->nominal_speed;
@@ -1305,6 +1286,53 @@ Having the real displacement of the head, we can calculate the total movement le
 
   // Precalculate the division, so when all the trapezoids in the planner queue get recalculated, the division is not repeated.
   block->speed_factor = block->nominal_rate / block->nominal_speed;
+
+#ifdef LIN_ADVANCE
+  if (block->use_advance_lead) {
+      // calculate the compression ratio for the segment (the required advance steps are computed
+      // during trapezoid planning)
+      float adv_comp = extruder_advance_K * e_D_ratio * cs.axis_steps_per_unit[E_AXIS]; // (step/(mm/s))
+      block->adv_comp = adv_comp / block->speed_factor; // step/(step/min)
+
+      float advance_speed;
+      if (e_D_ratio > 0)
+          advance_speed = (extruder_advance_K * e_D_ratio * block->acceleration * cs.axis_steps_per_unit[E_AXIS]);
+      else
+          advance_speed = cs.max_jerk[E_AXIS] * cs.axis_steps_per_unit[E_AXIS];
+
+      // to save more space we avoid another copy of calc_timer and go through slow division, but we
+      // still need to replicate the *exact* same step grouping policy (see below)
+      if (advance_speed > MAX_STEP_FREQUENCY) advance_speed = MAX_STEP_FREQUENCY;
+      float advance_rate = (F_CPU / 8.0) / advance_speed;
+      if (advance_speed > 20000) {
+          block->advance_rate = advance_rate * 4;
+          block->advance_step_loops = 4;
+      }
+      else if (advance_speed > 10000) {
+          block->advance_rate = advance_rate * 2;
+          block->advance_step_loops = 2;
+      }
+      else
+      {
+          // never overflow the internal accumulator with very low rates
+          if (advance_rate < UINT16_MAX)
+              block->advance_rate = advance_rate;
+          else
+              block->advance_rate = UINT16_MAX;
+          block->advance_step_loops = 1;
+      }
+
+      #ifdef LA_DEBUG
+      if (block->advance_step_loops > 2)
+          // @wavexx: we should really check for the difference between step_loops and
+          //          advance_step_loops instead. A difference of more than 1 will lead
+          //          to uneven speed and *should* be adjusted here by furthermore
+          //          reducing the speed.
+          SERIAL_ECHOLNPGM("LA: More than 2 steps per eISR loop executed.");
+      #endif
+  }
+#endif
+
   calculate_trapezoid_for_block(block, block->entry_speed, safe_speed);
 
   if (block->step_event_count.wide <= 32767)

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;
@@ -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;
@@ -736,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
 
@@ -812,8 +801,11 @@ FORCE_INLINE void isr() {
         acceleration_time += timer;
 #ifdef LIN_ADVANCE
         if (current_block->use_advance_lead) {
-            if (step_events_completed.wide <= (unsigned long int)step_loops)
+            if (step_events_completed.wide <= (unsigned long int)step_loops) {
                 la_state = ADV_INIT | ADV_ACC_VARY;
+                if (e_extruding && current_adv_steps > target_adv_steps)
+                    target_adv_steps = current_adv_steps;
+            }
         }
 #endif
       }
@@ -835,6 +827,8 @@ FORCE_INLINE void isr() {
             if (step_events_completed.wide <= (unsigned long int)current_block->decelerate_after + step_loops) {
                 target_adv_steps = current_block->final_adv_steps;
                 la_state = ADV_INIT | ADV_ACC_VARY;
+                if (e_extruding && current_adv_steps < target_adv_steps)
+                    target_adv_steps = current_adv_steps;
             }
         }
 #endif
@@ -848,12 +842,12 @@ FORCE_INLINE void isr() {
 
 #ifdef LIN_ADVANCE
           if(current_block->use_advance_lead) {
-              if (!nextAdvanceISR) {
-                  // Due to E-jerk, there can be discontinuities in pressure state where an
-                  // acceleration or deceleration can be skipped or joined with the previous block.
-                  // If LA was not previously active, re-check the pressure level
-                  la_state = ADV_INIT;
-              }
+              // Due to E-jerk, there can be discontinuities in pressure state where an
+              // acceleration or deceleration can be skipped or joined with the previous block.
+              // If LA was not previously active, re-check the pressure level
+              la_state = ADV_INIT;
+              if (e_extruding)
+                  target_adv_steps = current_adv_steps;
           }
 #endif
         }
@@ -865,14 +859,21 @@ FORCE_INLINE void isr() {
 #ifdef LIN_ADVANCE
     // avoid multiple instances or function calls to advance_spread
     if (la_state & ADV_INIT) {
+        LA_phase = -1;
+
         if (current_adv_steps == target_adv_steps) {
-            // nothing to be done in this phase
+            // nothing to be done in this phase, cancel any pending eisr
             la_state = 0;
+            nextAdvanceISR = ADV_NEVER;
         }
         else {
-            eISR_Err = current_block->advance_rate / 4;
+            // reset error and iterations per loop for this phase
+            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;
             }
         }
@@ -882,11 +883,13 @@ FORCE_INLINE void isr() {
         advance_spread(main_Rate);
         if (LA_phase >= 0) {
             if (step_loops == e_step_loops)
-                LA_phase = (eISR_Rate > main_Rate);
+                LA_phase = (current_block->advance_rate < main_Rate);
             else {
                 // avoid overflow through division. warning: we need to _guarantee_ step_loops
                 // and e_step_loops are <= 4 due to fastdiv's limit
-                LA_phase = (fastdiv(eISR_Rate, step_loops) > fastdiv(main_Rate, e_step_loops));
+                auto adv_rate_n = fastdiv(current_block->advance_rate, step_loops);
+                auto main_rate_n = fastdiv(main_Rate, e_step_loops);
+                LA_phase = (adv_rate_n < main_rate_n);
             }
         }
     }
@@ -928,26 +931,34 @@ 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;
-        nextAdvanceISR = eISR_Rate;
+        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;
-        nextAdvanceISR = eISR_Rate;
     }
-    else {
+
+    if (current_adv_steps == target_adv_steps) {
         // advance steps completed
         nextAdvanceISR = ADV_NEVER;
-        LA_phase = -1;
-        e_step_loops = 1;
+    }
+    else {
+        // schedule another tick
+        nextAdvanceISR = eISR_Rate;
     }
 }
 
@@ -1017,7 +1028,7 @@ FORCE_INLINE void advance_isr_scheduler() {
 
     // Schedule the next closest tick, ignoring advance if scheduled too
     // soon in order to avoid skewing the regular stepper acceleration
-    if (nextAdvanceISR != ADV_NEVER && (nextAdvanceISR + TCNT1 + 40) < nextMainISR)
+    if (nextAdvanceISR != ADV_NEVER && (nextAdvanceISR + 40) < nextMainISR)
         OCR1A = nextAdvanceISR;
     else
         OCR1A = nextMainISR;