diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 8aba9ddc1..37d2adf20 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -672,8 +672,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
   #endif
 
   #if ENABLED(LIN_ADVANCE)
-    const float target_float[XYZE] = { a, b, c, e },
-                mm_D_float = sqrt(sq(target_float[X_AXIS] - position_float[X_AXIS]) + sq(target_float[Y_AXIS] - position_float[Y_AXIS]));
+    const float mm_D_float = sqrt(sq(a - position_float[X_AXIS]) + sq(b - position_float[Y_AXIS]));
   #endif
 
   const long da = target[X_AXIS] - position[X_AXIS],
@@ -707,13 +706,14 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
   if (DEBUGGING(DRYRUN)) {
     position[E_AXIS] = target[E_AXIS];
     #if ENABLED(LIN_ADVANCE)
-      position_float[E_AXIS] = target_float[E_AXIS];
+      position_float[E_AXIS] = e;
     #endif
   }
 
   long de = target[E_AXIS] - position[E_AXIS];
+
   #if ENABLED(LIN_ADVANCE)
-    float de_float = target_float[E_AXIS] - position_float[E_AXIS];
+    float de_float = e - position_float[E_AXIS];
   #endif
 
   #if ENABLED(PREVENT_COLD_EXTRUSION)
@@ -722,7 +722,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
         position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
         de = 0; // no difference
         #if ENABLED(LIN_ADVANCE)
-          position_float[E_AXIS] = target_float[E_AXIS];
+          position_float[E_AXIS] = e;
           de_float = 0;
         #endif
         SERIAL_ECHO_START;
@@ -733,7 +733,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
           position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
           de = 0; // no difference
           #if ENABLED(LIN_ADVANCE)
-            position_float[E_AXIS] = target_float[E_AXIS];
+            position_float[E_AXIS] = e;
             de_float = 0;
           #endif
           SERIAL_ECHO_START;
@@ -1356,7 +1356,10 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
   // Update the position (only when a move was queued)
   memcpy(position, target, sizeof(position));
   #if ENABLED(LIN_ADVANCE)
-    memcpy(position_float, target_float, sizeof(position_float));
+    position_float[X_AXIS] = a;
+    position_float[Y_AXIS] = b;
+    position_float[Z_AXIS] = c;
+    position_float[E_AXIS] = e;
   #endif
 
   recalculate();
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index ddab5416e..da2ee14cc 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -342,13 +342,14 @@ ISR(TIMER1_COMPA_vect) {
   #endif
 }
 
-void Stepper::isr() {
-  #define _ENABLE_ISRs() cli(); SBI(TIMSK0, OCIE0B); ENABLE_STEPPER_DRIVER_INTERRUPT()
+#define _ENABLE_ISRs() do { cli(); if (thermalManager.in_temp_isr) CBI(TIMSK0, OCIE0B); else SBI(TIMSK0, OCIE0B); ENABLE_STEPPER_DRIVER_INTERRUPT(); } while(0)
 
-  uint16_t timer, remainder, ocr_val;
+void Stepper::isr() {
 
   static uint32_t step_remaining = 0;
 
+  uint16_t ocr_val;
+
   #define ENDSTOP_NOMINAL_OCR_VAL 3000    // check endstops every 1.5ms to guarantee two stepper ISRs within 5ms for BLTouch
   #define OCR_VAL_TOLERANCE 1000          // First max delay is 2.0ms, last min delay is 0.5ms, all others 1.5ms
 
@@ -366,7 +367,7 @@ void Stepper::isr() {
     #define SPLIT(L) do { \
       _SPLIT(L); \
       if (ENDSTOPS_ENABLED && L > ENDSTOP_NOMINAL_OCR_VAL) { \
-        remainder = (uint16_t)L % (ENDSTOP_NOMINAL_OCR_VAL); \
+        uint16_t remainder = (uint16_t)L % (ENDSTOP_NOMINAL_OCR_VAL); \
         ocr_val = (remainder < OCR_VAL_TOLERANCE) ? ENDSTOP_NOMINAL_OCR_VAL + remainder : ENDSTOP_NOMINAL_OCR_VAL; \
         step_remaining = (uint16_t)L - ocr_val; \
       } \
@@ -374,13 +375,16 @@ void Stepper::isr() {
 
     if (step_remaining && ENDSTOPS_ENABLED) {   // Just check endstops - not yet time for a step
       endstops.update();
-      ocr_val = step_remaining;
       if (step_remaining > ENDSTOP_NOMINAL_OCR_VAL) {
-        step_remaining = step_remaining - ENDSTOP_NOMINAL_OCR_VAL;
+        step_remaining -= ENDSTOP_NOMINAL_OCR_VAL;
         ocr_val = ENDSTOP_NOMINAL_OCR_VAL;
       }
-      else step_remaining = 0;  //  last one before the ISR that does the step
-      _NEXT_ISR(ocr_val);  //
+      else {
+        ocr_val = step_remaining;
+        step_remaining = 0;  //  last one before the ISR that does the step
+      }
+
+      _NEXT_ISR(ocr_val);
 
       NOLESS(OCR1A, TCNT1 + 16);
 
@@ -867,9 +871,7 @@ void Stepper::isr() {
     NOLESS(OCR1A, TCNT1 + 16);
 
     // Restore original ISR settings
-    cli();
-    SBI(TIMSK0, OCIE0B);
-    ENABLE_STEPPER_DRIVER_INTERRUPT();
+    _ENABLE_ISRs();
   }
 
 #endif // ADVANCE or LIN_ADVANCE