Merge pull request #15 from prusa3d/MK3-michal

Linear Advance

Firmware/Configuration_adv.h

@@ -265,24 +265,45 @@
   #endif
 #endif
 
-// extruder advance constant (s2/mm3)
-//
-// advance (steps) = STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K * cubic mm per second ^ 2
-//
-// Hooke's law says:		force = k * distance
-// Bernoulli's principle says:	v ^ 2 / 2 + g . h + pressure / density = constant
-// so: v ^ 2 is proportional to number of steps we advance the extruder
-//#define ADVANCE
-
-#ifdef ADVANCE
-  #define EXTRUDER_ADVANCE_K .006
-
-  #define D_FILAMENT 1.75
-  #define STEPS_MM_E 174.6
-  #define EXTRUSION_AREA (0.25 * D_FILAMENT * D_FILAMENT * 3.14159)
-  #define STEPS_PER_CUBIC_MM_E (axis_steps_per_unit[E_AXIS]/ EXTRUSION_AREA)
-
-#endif // ADVANCE
+/**
+    * Implementation of linear pressure control
+    *
+    * Assumption: advance = k * (delta velocity)
+    * K=0 means advance disabled.
+    * See Marlin documentation for calibration instructions.
+    */
+#define LIN_ADVANCE
+
+#ifdef LIN_ADVANCE
+  #define LIN_ADVANCE_K 0 //Try around 45 for PLA, around 25 for ABS.
+
+ /**
+        * Some Slicers produce Gcode with randomly jumping extrusion widths occasionally.
+        * For example within a 0.4mm perimeter it may produce a single segment of 0.05mm width.
+        * While this is harmless for normal printing (the fluid nature of the filament will
+        * close this very, very tiny gap), it throws off the LIN_ADVANCE pressure adaption.
+        *
+        * For this case LIN_ADVANCE_E_D_RATIO can be used to set the extrusion:distance ratio
+        * to a fixed value. Note that using a fixed ratio will lead to wrong nozzle pressures
+        * if the slicer is using variable widths or layer heights within one print!
+        *
+        * This option sets the default E:D ratio at startup. Use `M900` to override this value.
+        *
+        * Example: `M900 W0.4 H0.2 D1.75`, where:
+        *   - W is the extrusion width in mm
+        *   - H is the layer height in mm
+        *   - D is the filament diameter in mm
+        *
+        * Example: `M900 R0.0458` to set the ratio directly.
+        *
+        * Set to 0 to auto-detect the ratio based on given Gcode G1 print moves.
+        *
+        * Slic3r (including Prusa Slic3r) produces Gcode compatible with the automatic mode.
+        * Cura (as of this writing) may produce Gcode incompatible with the automatic mode.
+        */
+#define LIN_ADVANCE_E_D_RATIO 0 // The calculated ratio (or 0) according to the formula W * H / ((D / 2) ^ 2 * PI)
+                                // Example: 0.4 * 0.2 / ((1.75 / 2) ^ 2 * PI) = 0.033260135
+#endif
 
 // Arc interpretation settings:
 #define MM_PER_ARC_SEGMENT 1

Firmware/Marlin_main.cpp

@@ -55,10 +55,10 @@
 #include "math.h"
 #include "util.h"
 
-#include <avr/wdt.h>
+#include <avr/wdt.h>
 
 #ifdef HAVE_PAT9125_SENSOR
-#include "swspi.h"
+#include "swspi.h"
 #include "pat9125.h"
 #endif //HAVE_PAT9125_SENSOR
 
@@ -210,6 +210,7 @@
 // M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
 // M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
 // M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ]
+// M900 - Set LIN_ADVANCE options, if enabled. See Configuration_adv.h for details.
 // M907 - Set digital trimpot motor current using axis codes.
 // M908 - Control digital trimpot directly.
 // M350 - Set microstepping mode.
@@ -910,86 +911,86 @@ static void lcd_language_menu();
 
 #ifdef HAVE_PAT9125_SENSOR
 
-bool fsensor_enabled = true;
-bool fsensor_ignore_error = true;
-bool fsensor_M600 = false;
-long prev_pos_e = 0;
-long err_cnt = 0;
-
-#define FSENS_ESTEPS 140  //extruder resolution [steps/mm]
-#define FSENS_MINDEL 280  //filament sensor min delta [steps] (3mm)
-#define FSENS_MINFAC 3    //filament sensor minimum factor [count/mm]
-#define FSENS_MAXFAC 50   //filament sensor maximum factor [count/mm]
-#define FSENS_MAXERR 2    //filament sensor max error count
-
-void fsensor_enable()
-{
+bool fsensor_enabled = true;
+bool fsensor_ignore_error = true;
+bool fsensor_M600 = false;
+long prev_pos_e = 0;
+long err_cnt = 0;
+
+#define FSENS_ESTEPS 140  //extruder resolution [steps/mm]
+#define FSENS_MINDEL 280  //filament sensor min delta [steps] (3mm)
+#define FSENS_MINFAC 3    //filament sensor minimum factor [count/mm]
+#define FSENS_MAXFAC 50   //filament sensor maximum factor [count/mm]
+#define FSENS_MAXERR 2    //filament sensor max error count
+
+void fsensor_enable()
+{
 	MYSERIAL.println("fsensor_enable");
-	pat9125_y = 0;
-	prev_pos_e = st_get_position(E_AXIS);
-	err_cnt = 0;
-	fsensor_enabled = true;
-	fsensor_ignore_error = true;
-	fsensor_M600 = false;
-}
-
-void fsensor_disable()
-{
+	pat9125_y = 0;
+	prev_pos_e = st_get_position(E_AXIS);
+	err_cnt = 0;
+	fsensor_enabled = true;
+	fsensor_ignore_error = true;
+	fsensor_M600 = false;
+}
+
+void fsensor_disable()
+{
 	MYSERIAL.println("fsensor_disable");
-	fsensor_enabled = false;
-}
-
-void fsensor_update()
-{
-	if (!fsensor_enabled) return;
-	long pos_e = st_get_position(E_AXIS); //current position
-	pat9125_update();
-	long del_e = pos_e - prev_pos_e; //delta
-	if (abs(del_e) < FSENS_MINDEL) return;
-	float de = ((float)del_e / FSENS_ESTEPS);
-	int cmin = de * FSENS_MINFAC;
-	int cmax = de * FSENS_MAXFAC;
-	int cnt = pat9125_y;
-	prev_pos_e = pos_e;
-	pat9125_y = 0;
-	bool err = false;
-	if ((del_e > 0) && ((cnt < cmin) || (cnt > cmax))) err = true;
-	if ((del_e < 0) && ((cnt > cmin) || (cnt < cmax))) err = true;
-	if (err)
-		err_cnt++;
-	else
-		err_cnt = 0;
-
-/*
-	MYSERIAL.print("de=");
-	MYSERIAL.print(de);
-	MYSERIAL.print(" cmin=");
-	MYSERIAL.print((int)cmin);
-	MYSERIAL.print(" cmax=");
-	MYSERIAL.print((int)cmax);
-	MYSERIAL.print(" cnt=");
-	MYSERIAL.print((int)cnt);
-	MYSERIAL.print(" err=");
-	MYSERIAL.println((int)err_cnt);*/
-
-	if (err_cnt > FSENS_MAXERR)
-	{
+	fsensor_enabled = false;
+}
+
+void fsensor_update()
+{
+	if (!fsensor_enabled) return;
+	long pos_e = st_get_position(E_AXIS); //current position
+	pat9125_update();
+	long del_e = pos_e - prev_pos_e; //delta
+	if (abs(del_e) < FSENS_MINDEL) return;
+	float de = ((float)del_e / FSENS_ESTEPS);
+	int cmin = de * FSENS_MINFAC;
+	int cmax = de * FSENS_MAXFAC;
+	int cnt = pat9125_y;
+	prev_pos_e = pos_e;
+	pat9125_y = 0;
+	bool err = false;
+	if ((del_e > 0) && ((cnt < cmin) || (cnt > cmax))) err = true;
+	if ((del_e < 0) && ((cnt > cmin) || (cnt < cmax))) err = true;
+	if (err)
+		err_cnt++;
+	else
+		err_cnt = 0;
+
+/*
+	MYSERIAL.print("de=");
+	MYSERIAL.print(de);
+	MYSERIAL.print(" cmin=");
+	MYSERIAL.print((int)cmin);
+	MYSERIAL.print(" cmax=");
+	MYSERIAL.print((int)cmax);
+	MYSERIAL.print(" cnt=");
+	MYSERIAL.print((int)cnt);
+	MYSERIAL.print(" err=");
+	MYSERIAL.println((int)err_cnt);*/
+
+	if (err_cnt > FSENS_MAXERR)
+	{
 		MYSERIAL.println("fsensor_update (err_cnt > FSENS_MAXERR)");
-		if (fsensor_ignore_error)
-		{
+		if (fsensor_ignore_error)
+		{
 			MYSERIAL.println("fsensor_update - error ignored)");
-			fsensor_ignore_error = false;
-		}
-		else
-		{
+			fsensor_ignore_error = false;
+		}
+		else
+		{
 			MYSERIAL.println("fsensor_update - ERROR!!!");
 			planner_abort_hard();
 //			planner_pause_and_save();
-			enquecommand_front_P((PSTR("M600")));
-			fsensor_M600 = true;
-			fsensor_enabled = false;
-		}
-	}
+			enquecommand_front_P((PSTR("M600")));
+			fsensor_M600 = true;
+			fsensor_enabled = false;
+		}
+	}
 }
 
 #endif //HAVE_PAT9125_SENSOR
@@ -1755,6 +1756,15 @@ static inline long    code_value_long()    { return strtol(strchr_pointer+1, NUL
 static inline int16_t code_value_short()   { return int16_t(strtol(strchr_pointer+1, NULL, 10)); };
 static inline uint8_t code_value_uint8()   { return uint8_t(strtol(strchr_pointer+1, NULL, 10)); };
 
+static inline float code_value_float() {
+    char* e = strchr(strchr_pointer, 'E');
+    if (!e) return strtod(strchr_pointer + 1, NULL);
+    *e = 0;
+    float ret = strtod(strchr_pointer + 1, NULL);
+    *e = 'E';
+    return ret;
+}
+
 #define DEFINE_PGM_READ_ANY(type, reader)       \
     static inline type pgm_read_any(const type *p)  \
     { return pgm_read_##reader##_near(p); }
@@ -1940,6 +1950,40 @@ static float probe_pt(float x, float y, float z_before) {
 
 #endif // #ifdef ENABLE_AUTO_BED_LEVELING
 
+
+#ifdef LIN_ADVANCE
+   /**
+    * M900: Set and/or Get advance K factor and WH/D ratio
+    *
+    *  K<factor>                  Set advance K factor
+    *  R<ratio>                   Set ratio directly (overrides WH/D)
+    *  W<width> H<height> D<diam> Set ratio from WH/D
+    */
+inline void gcode_M900() {
+    st_synchronize();
+    
+    const float newK = code_seen('K') ? code_value_float() : -1;
+    if (newK >= 0) extruder_advance_k = newK;
+    
+    float newR = code_seen('R') ? code_value_float() : -1;
+    if (newR < 0) {
+        const float newD = code_seen('D') ? code_value_float() : -1,
+        newW = code_seen('W') ? code_value_float() : -1,
+        newH = code_seen('H') ? code_value_float() : -1;
+        if (newD >= 0 && newW >= 0 && newH >= 0)
+            newR = newD ? (newW * newH) / (sq(newD * 0.5) * M_PI) : 0;
+    }
+    if (newR >= 0) advance_ed_ratio = newR;
+    
+    SERIAL_ECHO_START;
+    SERIAL_ECHOPGM("Advance K=");
+    SERIAL_ECHOLN(extruder_advance_k);
+    SERIAL_ECHOPGM(" E/D=");
+    const float ratio = advance_ed_ratio;
+    if (ratio) SERIAL_ECHOLN(ratio); else SERIAL_ECHOLNPGM("Auto");
+    }
+#endif // LIN_ADVANCE
+
 void homeaxis(int axis) {
 #define HOMEAXIS_DO(LETTER) \
 ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
@@ -5567,6 +5611,12 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 		if(lcd_commands_type == 0)	lcd_commands_type = LCD_COMMAND_LONG_PAUSE_RESUME;
 	}
 	break;
+            
+#ifdef LIN_ADVANCE
+    case 900: // M900: Set LIN_ADVANCE options.
+        gcode_M900();
+    break;
+#endif
 
     case 907: // M907 Set digital trimpot motor current using axis codes.
     {
@@ -5793,6 +5843,12 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 		  }
 		  snmm_filaments_used |= (1 << tmp_extruder); //for stop print
 #ifdef SNMM
+          
+    #ifdef LIN_ADVANCE
+          if (snmm_extruder != tmp_extruder)
+            clear_current_adv_vars(); //Check if the selected extruder is not the active one and reset LIN_ADVANCE variables if so.
+    #endif
+          
 		  snmm_extruder = tmp_extruder;
 
 		  st_synchronize();
@@ -5883,66 +5939,66 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 	  }
   } // end if(code_seen('T')) (end of T codes)
 
-  else if (code_seen('D')) // D codes (debug)
-  {
-    switch((int)code_value())
-    {
-	case 0: // D0 - Reset
-		MYSERIAL.println("D0 - Reset");
-		cli(); //disable interrupts
-		wdt_reset(); //reset watchdog
-		WDTCSR = (1<<WDCE) | (1<<WDE); //enable watchdog
-		WDTCSR = (1<<WDE) | (1<<WDP0); //30ms prescaler
-		while(1); //wait for reset
-		break;
-	case 1: // D1 - Clear EEPROM
-		{
-			MYSERIAL.println("D1 - Clear EEPROM");
-			cli();
-			for (int i = 0; i < 4096; i++)
-				eeprom_write_byte((unsigned char*)i, (unsigned char)0);
-			sei();
-		}
-		break;
-	case 2: // D2 - read/write PIN
-		{
-			if (code_seen('P')) // Pin (0-255)
-			{
-				int pin = (int)code_value();
-				if ((pin >= 0) && (pin <= 255))
-				{
-					if (code_seen('F')) // Function in/out (0/1)
-					{
-						int fnc = (int)code_value();
-						if (fnc == 0) pinMode(pin, INPUT);
-						else if (fnc == 1) pinMode(pin, OUTPUT);
-					}
-					if (code_seen('V')) // Value (0/1)
-					{
-						int val = (int)code_value();
-						if (val == 0) digitalWrite(pin, LOW);
-						else if (val == 1) digitalWrite(pin, HIGH);
-					}
-					else
-					{
-						int val = (digitalRead(pin) != LOW)?1:0;
-						MYSERIAL.print("PIN");
-						MYSERIAL.print(pin);
-						MYSERIAL.print("=");
-						MYSERIAL.println(val);
-					}
-				}
-			}
-		}
-		break;
-	case 3:
-		fsensor_enable();
-		break;
-	case 4:
-		fsensor_disable();
-		break;
-	}
-  }
+  else if (code_seen('D')) // D codes (debug)
+  {
+    switch((int)code_value())
+    {
+	case 0: // D0 - Reset
+		MYSERIAL.println("D0 - Reset");
+		cli(); //disable interrupts
+		wdt_reset(); //reset watchdog
+		WDTCSR = (1<<WDCE) | (1<<WDE); //enable watchdog
+		WDTCSR = (1<<WDE) | (1<<WDP0); //30ms prescaler
+		while(1); //wait for reset
+		break;
+	case 1: // D1 - Clear EEPROM
+		{
+			MYSERIAL.println("D1 - Clear EEPROM");
+			cli();
+			for (int i = 0; i < 4096; i++)
+				eeprom_write_byte((unsigned char*)i, (unsigned char)0);
+			sei();
+		}
+		break;
+	case 2: // D2 - read/write PIN
+		{
+			if (code_seen('P')) // Pin (0-255)
+			{
+				int pin = (int)code_value();
+				if ((pin >= 0) && (pin <= 255))
+				{
+					if (code_seen('F')) // Function in/out (0/1)
+					{
+						int fnc = (int)code_value();
+						if (fnc == 0) pinMode(pin, INPUT);
+						else if (fnc == 1) pinMode(pin, OUTPUT);
+					}
+					if (code_seen('V')) // Value (0/1)
+					{
+						int val = (int)code_value();
+						if (val == 0) digitalWrite(pin, LOW);
+						else if (val == 1) digitalWrite(pin, HIGH);
+					}
+					else
+					{
+						int val = (digitalRead(pin) != LOW)?1:0;
+						MYSERIAL.print("PIN");
+						MYSERIAL.print(pin);
+						MYSERIAL.print("=");
+						MYSERIAL.println(val);
+					}
+				}
+			}
+		}
+		break;
+	case 3:
+		fsensor_enable();
+		break;
+	case 4:
+		fsensor_disable();
+		break;
+	}
+  }
 
   else
   {

Firmware/planner.cpp

@@ -126,6 +126,12 @@ float extrude_min_temp=EXTRUDE_MINTEMP;
  static char meas_sample; //temporary variable to hold filament measurement sample
 #endif
 
+#ifdef LIN_ADVANCE
+    float extruder_advance_k = LIN_ADVANCE_K,
+    advance_ed_ratio = LIN_ADVANCE_E_D_RATIO,
+    position_float[NUM_AXIS] = { 0 };
+#endif
+
 // Returns the index of the next block in the ring buffer
 // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
 static inline int8_t next_block_index(int8_t block_index) {
@@ -411,6 +417,9 @@ void plan_init() {
   block_buffer_head = 0;
   block_buffer_tail = 0;
   memset(position, 0, sizeof(position)); // clear position
+#ifdef LIN_ADVANCE
+  memset(position_float, 0, sizeof(position)); // clear position
+#endif
   previous_speed[0] = 0.0;
   previous_speed[1] = 0.0;
   previous_speed[2] = 0.0;
@@ -676,12 +685,22 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
     target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
 #endif // ENABLE_MESH_BED_LEVELING
   target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
+  
+#ifdef LIN_ADVANCE
+    const float mm_D_float = sqrt(sq(x - position_float[X_AXIS]) + sq(y - position_float[Y_AXIS]));
+    float de_float = e - position_float[E_AXIS];
+#endif
+    
   #ifdef PREVENT_DANGEROUS_EXTRUDE
   if(target[E_AXIS]!=position[E_AXIS])
   {
     if(degHotend(active_extruder)<extrude_min_temp)
     {
       position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
+#ifdef LIN_ADVANCE
+      position_float[E_AXIS] = e;
+      de_float = 0;
+#endif
       SERIAL_ECHO_START;
       SERIAL_ECHOLNRPGM(MSG_ERR_COLD_EXTRUDE_STOP);
     }
@@ -690,6 +709,10 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
     if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
     {
       position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
+#ifdef LIN_ADVANCE
+        position_float[E_AXIS] = e;
+        de_float = 0;
+#endif
       SERIAL_ECHO_START;
       SERIAL_ECHOLNRPGM(MSG_ERR_LONG_EXTRUDE_STOP);
     }
@@ -1112,6 +1135,37 @@ Having the real displacement of the head, we can calculate the total movement le
   previous_nominal_speed = block->nominal_speed;
   previous_safe_speed = safe_speed;
 
+#ifdef LIN_ADVANCE
+
+    //
+    // Use LIN_ADVANCE for blocks if all these are true:
+    //
+    // esteps                                          : We have E steps todo (a printing move)
+    //
+    // block->steps[X_AXIS] || block->steps[Y_AXIS]    : We have a movement in XY direction (i.e., not retract / prime).
+    //
+    // extruder_advance_k                              : There is an advance factor set.
+    //
+    // block->steps[E_AXIS] != block->step_event_count : A problem occurs if the move before a retract is too small.
+    //                                                   In that case, the retract and move will be executed together.
+    //                                                   This leads to too many advance steps due to a huge e_acceleration.
+    //                                                   The math is good, but we must avoid retract moves with advance!
+    // de_float > 0.0                                  : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
+    //
+    block->use_advance_lead =  block->steps_e
+                           && (block->steps_x || block->steps_y)
+                           && extruder_advance_k
+                           && (uint32_t)block->steps_e != block->step_event_count
+                           && de_float > 0.0;
+    if (block->use_advance_lead)
+        block->abs_adv_steps_multiplier8 = lround(
+                          extruder_advance_k
+                          * ((advance_ed_ratio < 0.000001) ? de_float / mm_D_float : advance_ed_ratio) // Use the fixed ratio, if set
+                          * (block->nominal_speed / (float)block->nominal_rate)
+                          * axis_steps_per_unit[E_AXIS] * 256.0
+                          );
+#endif
+    
   // 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;
   calculate_trapezoid_for_block(block, block->entry_speed, safe_speed);
@@ -1122,6 +1176,13 @@ Having the real displacement of the head, we can calculate the total movement le
   // Update position
   memcpy(position, target, sizeof(target)); // position[] = target[]
 
+#ifdef LIN_ADVANCE
+  position_float[X_AXIS] = x;
+  position_float[Y_AXIS] = y;
+  position_float[Z_AXIS] = z;
+  position_float[E_AXIS] = e;
+#endif
+    
   // Recalculate the trapezoids to maximize speed at the segment transitions while respecting
   // the machine limits (maximum acceleration and maximum jerk).
   // This runs asynchronously with the stepper interrupt controller, which may
@@ -1178,7 +1239,13 @@ void plan_set_position(float x, float y, float z, const float &e)
 #else
   position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
 #endif // ENABLE_MESH_BED_LEVELING
-  position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);  
+  position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
+#ifdef LIN_ADVANCE
+  position_float[X_AXIS] = x;
+  position_float[Y_AXIS] = y;
+  position_float[Z_AXIS] = z;
+  position_float[E_AXIS] = e;
+#endif
   st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]);
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
   previous_speed[0] = 0.0;

Firmware/planner.h

@@ -88,8 +88,17 @@ typedef struct {
 
   // Pre-calculated division for the calculate_trapezoid_for_block() routine to run faster.
   float speed_factor;
+    
+#ifdef LIN_ADVANCE
+  bool use_advance_lead;
+  unsigned long abs_adv_steps_multiplier8; // Factorised by 2^8 to avoid float
+#endif
 } block_t;
 
+#ifdef LIN_ADVANCE
+  extern float extruder_advance_k, advance_ed_ratio;
+#endif
+
 #ifdef ENABLE_AUTO_BED_LEVELING
 // this holds the required transform to compensate for bed level
 extern matrix_3x3 plan_bed_level_matrix;

Firmware/stepper.cpp

@@ -98,6 +98,26 @@ int8_t SilentMode;
 volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
 volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
 
+#ifdef LIN_ADVANCE
+
+  uint16_t ADV_NEVER = 65535;
+
+  static uint16_t nextMainISR = 0;
+  static uint16_t nextAdvanceISR = ADV_NEVER;
+  static uint16_t eISR_Rate = ADV_NEVER;
+
+  static volatile int e_steps; //Extrusion steps to be executed by the stepper
+  static int final_estep_rate; //Speed of extruder at cruising speed
+  static int current_estep_rate; //The current speed of the extruder
+  static int current_adv_steps; //The current pretension of filament expressed in steps
+
+  #define ADV_RATE(T, L) (e_steps ? (T) * (L) / abs(e_steps) : ADV_NEVER)
+  #define _NEXT_ISR(T) nextMainISR = T
+
+#else
+  #define _NEXT_ISR(T) OCR1A = T
+#endif
+
 //===========================================================================
 //=============================functions         ============================
 //===========================================================================
@@ -319,24 +339,28 @@ FORCE_INLINE void trapezoid_generator_reset() {
   step_loops_nominal = step_loops;
   acc_step_rate = current_block->initial_rate;
   acceleration_time = calc_timer(acc_step_rate);
-  OCR1A = acceleration_time;
-
-//    SERIAL_ECHO_START;
-//    SERIAL_ECHOPGM("advance :");
-//    SERIAL_ECHO(current_block->advance/256.0);
-//    SERIAL_ECHOPGM("advance rate :");
-//    SERIAL_ECHO(current_block->advance_rate/256.0);
-//    SERIAL_ECHOPGM("initial advance :");
-//  SERIAL_ECHO(current_block->initial_advance/256.0);
-//    SERIAL_ECHOPGM("final advance :");
-//    SERIAL_ECHOLN(current_block->final_advance/256.0);
+  _NEXT_ISR(acceleration_time);
+
+  #ifdef LIN_ADVANCE
+    if (current_block->use_advance_lead) {
+        current_estep_rate = ((unsigned long)acc_step_rate * current_block->abs_adv_steps_multiplier8) >> 17;
+        final_estep_rate = (current_block->nominal_rate * current_block->abs_adv_steps_multiplier8) >> 17;
+        }
+   #endif
 
 }
 
 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
 // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
-ISR(TIMER1_COMPA_vect)
-{
+ISR(TIMER1_COMPA_vect) {
+  #ifdef LIN_ADVANCE
+    advance_isr_scheduler();
+  #else
+      isr();
+  #endif
+}
+
+void isr() {
 	//if (UVLO) uvlo();
   // If there is no current block, attempt to pop one from the buffer
   if (current_block == NULL) {
@@ -355,13 +379,13 @@ ISR(TIMER1_COMPA_vect)
       #ifdef Z_LATE_ENABLE
         if(current_block->steps_z > 0) {
           enable_z();
-          OCR1A = 2000; //1ms wait
+          _NEXT_ISR(2000); //1ms wait
           return;
         }
       #endif
     }
     else {
-        OCR1A=2000; // 1kHz.
+        _NEXT_ISR(2000); // 1kHz.
     }
   }
 
@@ -582,6 +606,15 @@ ISR(TIMER1_COMPA_vect)
       MSerial.checkRx(); // Check for serial chars.
       #endif
 
+#ifdef LIN_ADVANCE
+        counter_e += current_block->steps_e;
+        if (counter_e > 0) {
+          counter_e -= current_block->step_event_count;
+          count_position[E_AXIS] += count_direction[E_AXIS];
+          ((out_bits&(1<<E_AXIS))!=0) ? --e_steps : ++e_steps;
+        }
+#endif
+        
         counter_x += current_block->steps_x;
         if (counter_x > 0) {
           WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
@@ -636,6 +669,7 @@ ISR(TIMER1_COMPA_vect)
         #endif
       }
 
+#ifndef LIN_ADVANCE
         counter_e += current_block->steps_e;
         if (counter_e > 0) {
           WRITE_E_STEP(!INVERT_E_STEP_PIN);
@@ -643,9 +677,21 @@ ISR(TIMER1_COMPA_vect)
           count_position[E_AXIS]+=count_direction[E_AXIS];
           WRITE_E_STEP(INVERT_E_STEP_PIN);
         }
+#endif
+        
       step_events_completed += 1;
       if(step_events_completed >= current_block->step_event_count) break;
     }
+#ifdef LIN_ADVANCE
+      if (current_block->use_advance_lead) {
+        const int delta_adv_steps = current_estep_rate - current_adv_steps;
+        current_adv_steps += delta_adv_steps;
+        e_steps += delta_adv_steps;
+      }
+      // If we have esteps to execute, fire the next advance_isr "now"
+      if (e_steps) nextAdvanceISR = 0;
+#endif
+        
     // Calculare new timer value
     unsigned short timer;
     unsigned short step_rate;
@@ -660,8 +706,15 @@ ISR(TIMER1_COMPA_vect)
 
       // step_rate to timer interval
       timer = calc_timer(acc_step_rate);
-      OCR1A = timer;
+      _NEXT_ISR(timer);
       acceleration_time += timer;
+        
+#ifdef LIN_ADVANCE
+        if (current_block->use_advance_lead) {
+         current_estep_rate = ((uint32_t)acc_step_rate * current_block->abs_adv_steps_multiplier8) >> 17;
+        }
+        eISR_Rate = ADV_RATE(timer, step_loops);
+#endif
     }
     else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {
       MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
@@ -679,11 +732,25 @@ ISR(TIMER1_COMPA_vect)
 
       // step_rate to timer interval
       timer = calc_timer(step_rate);
-      OCR1A = timer;
+      _NEXT_ISR(timer);
       deceleration_time += timer;
+        
+#ifdef LIN_ADVANCE
+        if (current_block->use_advance_lead) {
+          current_estep_rate = ((uint32_t)step_rate * current_block->abs_adv_steps_multiplier8) >> 17;
+        }
+        eISR_Rate = ADV_RATE(timer, step_loops);
+#endif
     }
     else {
-      OCR1A = OCR1A_nominal;
+#ifdef LIN_ADVANCE
+        if (current_block->use_advance_lead)
+          current_estep_rate = final_estep_rate;
+
+        eISR_Rate = ADV_RATE(OCR1A_nominal, step_loops_nominal);
+#endif
+
+      _NEXT_ISR(OCR1A_nominal);
       // ensure we're running at the correct step rate, even if we just came off an acceleration
       step_loops = step_loops_nominal;
     }
@@ -697,6 +764,69 @@ ISR(TIMER1_COMPA_vect)
   check_fans();
 }
 
+#ifdef LIN_ADVANCE
+      
+      // Timer interrupt for E. e_steps is set in the main routine.
+      
+void advance_isr() {
+  
+  nextAdvanceISR = eISR_Rate;
+  
+  if (e_steps) {
+      bool dir =
+#ifdef SNMM
+      ((e_steps < 0) == (snmm_extruder & 1))
+#else
+      (e_steps < 0)
+#endif
+      ? INVERT_E0_DIR : !INVERT_E0_DIR; //If we have SNMM, reverse every second extruder.
+      WRITE(E0_DIR_PIN, dir);
+      
+      for (uint8_t i = step_loops; e_steps && i--;) {
+          WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
+          e_steps < 0 ? ++e_steps : --e_steps;
+          WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
+      }
+  }
+}
+
+void advance_isr_scheduler() {
+  // Run main stepping ISR if flagged
+  if (!nextMainISR) isr();
+  
+  // Run Advance stepping ISR if flagged
+  if (!nextAdvanceISR) advance_isr();
+  
+  // Is the next advance ISR scheduled before the next main ISR?
+  if (nextAdvanceISR <= nextMainISR) {
+      // Set up the next interrupt
+      OCR1A = nextAdvanceISR;
+      // New interval for the next main ISR
+      if (nextMainISR) nextMainISR -= nextAdvanceISR;
+      // Will call Stepper::advance_isr on the next interrupt
+      nextAdvanceISR = 0;
+  }
+  else {
+      // The next main ISR comes first
+      OCR1A = nextMainISR;
+      // New interval for the next advance ISR, if any
+      if (nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
+          nextAdvanceISR -= nextMainISR;
+      // Will call Stepper::isr on the next interrupt
+      nextMainISR = 0;
+  }
+  
+  // Don't run the ISR faster than possible
+  if (OCR1A < TCNT1 + 16) OCR1A = TCNT1 + 16;
+}
+
+void clear_current_adv_vars() {
+  e_steps = 0; //Should be already 0 at an filament change event, but just to be sure..
+  current_adv_steps = 0;
+}
+
+#endif // LIN_ADVANCE
+      
 void st_init()
 {
 #ifdef HAVE_TMC2130_DRIVERS
@@ -897,6 +1027,11 @@ void st_init()
   TCNT1 = 0;
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 
+#ifdef LIN_ADVANCE
+    e_steps = 0;
+    current_adv_steps = 0;
+#endif
+    
   enable_endstops(true); // Start with endstops active. After homing they can be disabled
   sei();
 }

Firmware/stepper.h

@@ -44,6 +44,16 @@ extern bool abort_on_endstop_hit;
 // Initialize and start the stepper motor subsystem
 void st_init();
 
+// Interrupt Service Routines
+
+void isr();
+
+#ifdef LIN_ADVANCE
+  void advance_isr();
+  void advance_isr_scheduler();
+  void clear_current_adv_vars(); //Used to reset the built up pretension and remaining esteps on filament change.
+#endif
+
 // Block until all buffered steps are executed
 void st_synchronize();
 

Firmware/variants/1_75mm_MK3-EINY04-E3Dv6full.h

@@ -134,8 +134,8 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define TMC2130_SG_DELAY     10     // stallguard delay (temporary solution)
 
 //new settings is possible for vsense = 1
-#define TMC2130_CURRENTS_H {15, 15, 20, 30}  // default holding currents for all axes
-#define TMC2130_CURRENTS_R {15, 15, 30, 30}  // default running currents for all axes
+#define TMC2130_CURRENTS_H {15, 15, 20, 28}  // default holding currents for all axes
+#define TMC2130_CURRENTS_R {15, 15, 40, 28}  // default running currents for all axes
 
 //#define TMC2130_DEBUG
 //#define TMC2130_DEBUG_WR