Skip hysteresis check when temp is already close to target

To eliminate a long delay during pause, park, and filament change

Marlin/src/module/temperature.cpp

@@ -2716,7 +2716,7 @@ void Temperature::isr() {
       #endif
 
       float target_temp = -1.0, old_temp = 9999.0;
-      bool wants_to_cool = false;
+      bool wants_to_cool = false, first_loop = true;
       wait_for_heatup = true;
       millis_t now, next_temp_ms = 0, next_cool_check_ms = 0;
       do {
@@ -2759,7 +2759,10 @@ void Temperature::isr() {
 
           if (!residency_start_ms) {
             // Start the TEMP_RESIDENCY_TIME timer when we reach target temp for the first time.
-            if (temp_diff < TEMP_WINDOW) residency_start_ms = now;
+            if (temp_diff < TEMP_WINDOW) {
+              residency_start_ms = now;
+              if (first_loop) residency_start_ms += (TEMP_RESIDENCY_TIME) * 1000UL;
+            }
           }
           else if (temp_diff > TEMP_HYSTERESIS) {
             // Restart the timer whenever the temperature falls outside the hysteresis.
@@ -2786,6 +2789,8 @@ void Temperature::isr() {
           }
         #endif
 
+        first_loop = false;
+
       } while (wait_for_heatup && TEMP_CONDITIONS);
 
       if (wait_for_heatup) {
@@ -2828,7 +2833,7 @@ void Temperature::isr() {
       #endif
 
       float target_temp = -1, old_temp = 9999;
-      bool wants_to_cool = false;
+      bool wants_to_cool = false, first_loop = true;
       wait_for_heatup = true;
       millis_t now, next_temp_ms = 0, next_cool_check_ms = 0;
 
@@ -2883,6 +2888,7 @@ void Temperature::isr() {
           if (!residency_start_ms) {
             // Start the TEMP_BED_RESIDENCY_TIME timer when we reach target temp for the first time.
             if (temp_diff < TEMP_BED_WINDOW) residency_start_ms = now;
+            if (first_loop) residency_start_ms += (TEMP_BED_RESIDENCY_TIME) * 1000UL;
           }
           else if (temp_diff > TEMP_BED_HYSTERESIS) {
             // Restart the timer whenever the temperature falls outside the hysteresis.
@@ -2909,6 +2915,8 @@ void Temperature::isr() {
           }
         #endif
 
+        first_loop = false;
+
       } while (wait_for_heatup && TEMP_BED_CONDITIONS);
 
       if (wait_for_heatup) ui.reset_status();