From 4d9780a3c68471f6c4d2f423efad87024949469f Mon Sep 17 00:00:00 2001
From: PavelSindler <sindlerpa@gmail.com>
Date: Tue, 14 Nov 2017 13:57:32 +0100
Subject: [PATCH] Merged lin adv. fix from MK2

---
 Firmware/Marlin_main.cpp | 3 ++-
 Firmware/planner.cpp     | 6 ++++++
 Firmware/stepper.cpp     | 7 ++++---
 3 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index b520cdb2..8b834445 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -5644,6 +5644,7 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
   else if(code_seen('T'))
   {
 	  int index;
+	  st_synchronize();
 	  for (index = 1; *(strchr_pointer + index) == ' ' || *(strchr_pointer + index) == '\t'; index++);
 	   
 	  if ((*(strchr_pointer + index) < '0' || *(strchr_pointer + index) > '9') && *(strchr_pointer + index) != '?') {
@@ -5666,7 +5667,7 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
           
 		  snmm_extruder = tmp_extruder;
 
-		  st_synchronize();
+		  
 		  delay(100);
 
 		  disable_e0();
diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp
index dde401c8..207dd240 100644
--- a/Firmware/planner.cpp
+++ b/Firmware/planner.cpp
@@ -1292,12 +1292,18 @@ void plan_set_position(float x, float y, float z, const float &e)
 // Only useful in the bed leveling routine, when the mesh bed leveling is off.
 void plan_set_z_position(const float &z)
 {
+	#ifdef LIN_ADVANCE
+	position_float[Z_AXIS] = z;
+	#endif
     position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
     st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]);
 }
 
 void plan_set_e_position(const float &e)
 {
+  #ifdef LIN_ADVANCE
+  position_float[E_AXIS] = e;
+  #endif
   position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);  
   st_set_e_position(position[E_AXIS]);
 }
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index 06055eec..40720db1 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -846,9 +846,6 @@ void isr() {
       // 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
@@ -869,6 +866,10 @@ void advance_isr() {
 
       }
   }
+  else {
+    eISR_Rate = ADV_NEVER;
+  }
+  nextAdvanceISR = eISR_Rate;
 }
 
 void advance_isr_scheduler() {