Merge pull request #5124 from Sebastianv650/RCBugFix_LIN_ADV-Patch

BugFix for incorrect E-speed calculation

Marlin/Marlin_main.cpp

@@ -6988,7 +6988,7 @@ inline void gcode_M503() {
    */
   inline void gcode_M905() {
     stepper.synchronize();
-    stepper.advance_M905(code_seen('K') ? code_value_float() : -1.0);
+    planner.advance_M905(code_seen('K') ? code_value_float() : -1.0);
   }
 #endif
 

Marlin/planner.cpp

@@ -131,6 +131,11 @@ float Planner::previous_speed[NUM_AXIS],
   long Planner::axis_segment_time[2][3] = { {MAX_FREQ_TIME + 1, 0, 0}, {MAX_FREQ_TIME + 1, 0, 0} };
 #endif
 
+#if ENABLED(LIN_ADVANCE)
+  float Planner::extruder_advance_k = LIN_ADVANCE_K;
+  float Planner::position_float[NUM_AXIS] = { 0 };
+#endif
+
 /**
  * Class and Instance Methods
  */
@@ -140,6 +145,9 @@ Planner::Planner() { init(); }
 void Planner::init() {
   block_buffer_head = block_buffer_tail = 0;
   ZERO(position);
+  #if ENABLED(LIN_ADVANCE)
+    ZERO(position_float);
+  #endif
   ZERO(previous_speed);
   previous_nominal_speed = 0.0;
   #if ABL_PLANAR
@@ -605,6 +613,14 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
     lround(e * axis_steps_per_mm[E_AXIS])
   };
   
+  #if ENABLED(LIN_ADVANCE)
+    float target_float[XYZE] = {a, b, c, e};
+    float de_float = target_float[E_AXIS] - position_float[E_AXIS];
+    float mm_D_float = sqrt(sq(target_float[X_AXIS] - position_float[X_AXIS]) + sq(target_float[Y_AXIS] - position_float[Y_AXIS]));
+    
+    memcpy(position_float, target_float, sizeof(position_float));
+  #endif
+
   long da = target[X_AXIS] - position[X_AXIS],
        db = target[Y_AXIS] - position[Y_AXIS],
        dc = target[Z_AXIS] - position[Z_AXIS];
@@ -1232,12 +1248,12 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
     // This leads to an enormous number of advance steps due to a huge e_acceleration.
     // The math is correct, but you don't want a retract move done with advance!
     // So this situation is filtered out here.
-    if (!esteps || (!block->steps[X_AXIS] && !block->steps[Y_AXIS]) || stepper.get_advance_k() == 0 || (uint32_t)esteps == block->step_event_count) {
+    if (!esteps || (!block->steps[X_AXIS] && !block->steps[Y_AXIS]) || extruder_advance_k == 0.0 || (uint32_t)esteps == block->step_event_count) {
       block->use_advance_lead = false;
     }
     else {
       block->use_advance_lead = true;
-      block->e_speed_multiplier8 = (esteps << 8) / block->step_event_count;
+      block->abs_adv_steps_multiplier8 = lround(extruder_advance_k * (de_float / mm_D_float) * block->nominal_speed / (float)block->nominal_rate * axis_steps_per_mm[Z_AXIS] * 256.0);
     }
 
   #elif ENABLED(ADVANCE)
@@ -1354,3 +1370,14 @@ void Planner::refresh_positioning() {
   }
 
 #endif
+
+#if ENABLED(LIN_ADVANCE)
+
+  void Planner::advance_M905(const float &k) {
+    if (k >= 0.0) extruder_advance_k = k;
+    SERIAL_ECHO_START;
+    SERIAL_ECHOPAIR("Advance factor: ", extruder_advance_k);
+    SERIAL_EOL;
+  }
+
+#endif

Marlin/planner.h

@@ -95,7 +95,7 @@ typedef struct {
   // Advance extrusion
   #if ENABLED(LIN_ADVANCE)
     bool use_advance_lead;
-    int16_t e_speed_multiplier8; // Factorised by 2^8 to avoid float
+    uint32_t abs_adv_steps_multiplier8; // Factorised by 2^8 to avoid float
   #elif ENABLED(ADVANCE)
     int32_t advance_rate;
     volatile int32_t initial_advance;
@@ -197,6 +197,11 @@ class Planner {
       static long axis_segment_time[2][3];
     #endif
     
+    #if ENABLED(LIN_ADVANCE)
+      static float position_float[NUM_AXIS];
+      static float extruder_advance_k;
+    #endif
+
   public:
 
     /**
@@ -246,6 +251,10 @@ class Planner {
 
     #endif
     
+    #if ENABLED(LIN_ADVANCE)
+      void advance_M905(const float &k);
+    #endif
+
     /**
      * Planner::_buffer_line
      *

Marlin/stepper.cpp

@@ -96,8 +96,7 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
 
   #if ENABLED(LIN_ADVANCE)
     volatile int Stepper::e_steps[E_STEPPERS];
-    int Stepper::extruder_advance_k = LIN_ADVANCE_K,
+    int Stepper::final_estep_rate,
-        Stepper::final_estep_rate,
         Stepper::current_estep_rate[E_STEPPERS],
         Stepper::current_adv_steps[E_STEPPERS];
   #else
@@ -534,7 +533,7 @@ void Stepper::isr() {
 
   #if ENABLED(LIN_ADVANCE)
     if (current_block->use_advance_lead) {
-      int delta_adv_steps = (((long)extruder_advance_k * current_estep_rate[TOOL_E_INDEX]) >> 9) - current_adv_steps[TOOL_E_INDEX];
+      int delta_adv_steps = current_estep_rate[TOOL_E_INDEX] - current_adv_steps[TOOL_E_INDEX];
       current_adv_steps[TOOL_E_INDEX] += delta_adv_steps;
       #if ENABLED(MIXING_EXTRUDER)
         // Mixing extruders apply advance lead proportionally
@@ -572,9 +571,9 @@ void Stepper::isr() {
       if (current_block->use_advance_lead) {
         #if ENABLED(MIXING_EXTRUDER)
           MIXING_STEPPERS_LOOP(j)
-            current_estep_rate[j] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 8;
+            current_estep_rate[j] = ((uint32_t)acc_step_rate * current_block->abs_adv_steps_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 17;
         #else
-          current_estep_rate[TOOL_E_INDEX] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8) >> 8;
+          current_estep_rate[TOOL_E_INDEX] = ((uint32_t)acc_step_rate * current_block->abs_adv_steps_multiplier8) >> 17;
         #endif
       }
 
@@ -624,9 +623,9 @@ void Stepper::isr() {
       if (current_block->use_advance_lead) {
         #if ENABLED(MIXING_EXTRUDER)
           MIXING_STEPPERS_LOOP(j)
-            current_estep_rate[j] = ((uint32_t)step_rate * current_block->e_speed_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 8;
+            current_estep_rate[j] = ((uint32_t)step_rate * current_block->abs_adv_steps_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 17;
         #else
-          current_estep_rate[TOOL_E_INDEX] = ((uint32_t)step_rate * current_block->e_speed_multiplier8) >> 8;
+          current_estep_rate[TOOL_E_INDEX] = ((uint32_t)step_rate * current_block->abs_adv_steps_multiplier8) >> 17;
         #endif
       }
 
@@ -1350,14 +1349,3 @@ void Stepper::report_positions() {
   }
 
 #endif // HAS_MICROSTEPS
-
-#if ENABLED(LIN_ADVANCE)
-
-  void Stepper::advance_M905(const float &k) {
-    if (k >= 0) extruder_advance_k = k;
-    SERIAL_ECHO_START;
-    SERIAL_ECHOPAIR("Advance factor: ", extruder_advance_k);
-    SERIAL_EOL;
-  }
-
-#endif // LIN_ADVANCE

Marlin/stepper.h

@@ -109,7 +109,6 @@ class Stepper {
       static volatile unsigned char eISR_Rate;
       #if ENABLED(LIN_ADVANCE)
         static volatile int e_steps[E_STEPPERS];
-        static int extruder_advance_k;
         static int final_estep_rate;
         static int current_estep_rate[E_STEPPERS]; // Actual extruder speed [steps/s]
         static int current_adv_steps[E_STEPPERS];  // The amount of current added esteps due to advance.
@@ -277,11 +276,6 @@ class Stepper {
       return endstops_trigsteps[axis] * planner.steps_to_mm[axis];
     }
 
-    #if ENABLED(LIN_ADVANCE)
-      void advance_M905(const float &k);
-      FORCE_INLINE int get_advance_k() { return extruder_advance_k; }
-    #endif
-
   private:
 
     static FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
@@ -367,8 +361,8 @@ class Stepper {
       
       #if ENABLED(LIN_ADVANCE)
         if (current_block->use_advance_lead) {
-          current_estep_rate[current_block->active_extruder] = ((unsigned long)acc_step_rate * current_block->e_speed_multiplier8) >> 8;
+          current_estep_rate[current_block->active_extruder] = ((unsigned long)acc_step_rate * current_block->abs_adv_steps_multiplier8) >> 17;
-          final_estep_rate = (current_block->nominal_rate * current_block->e_speed_multiplier8) >> 8;
+          final_estep_rate = (current_block->nominal_rate * current_block->abs_adv_steps_multiplier8) >> 17;
         }
       #endif