From eb8af486d2e153b57a8e6d508333feb7e12d751c Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Sun, 4 Jun 2017 17:03:22 -0500
Subject: [PATCH] Ensure safe temperature for M600

---
 Marlin/Marlin_main.cpp | 24 ++++++++++++++++++++++--
 Marlin/ultralcd.cpp    |  4 +++-
 2 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index c3635ca8d..d88286c4d 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -5921,6 +5921,25 @@ inline void gcode_M17() {
     return true;
   }
 
+  static void ensure_safe_temperature() {
+    bool did_show = false;
+    wait_for_heatup = true;
+    while (wait_for_heatup) {
+      idle();
+      wait_for_heatup = false;
+      HOTEND_LOOP() {
+        if (thermalManager.degTargetHotend(e) && abs(thermalManager.degHotend(e) - thermalManager.degTargetHotend(e)) > 3) {
+          wait_for_heatup = true;
+          if (!did_show) { // Show "wait for heating"
+            lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_WAIT_FOR_NOZZLES_TO_HEAT);
+            did_show = true;
+          }
+          break;
+        }
+      }
+    }
+  }
+
   static void wait_for_filament_reload(int8_t max_beep_count = 0) {
     bool nozzle_timed_out = false;
 
@@ -5937,8 +5956,7 @@ inline void gcode_M17() {
           nozzle_timed_out |= thermalManager.is_heater_idle(e);
 
       #if ENABLED(ULTIPANEL)
-        if (nozzle_timed_out)
-          lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_CLICK_TO_HEAT_NOZZLE);
+        if (nozzle_timed_out) ensure_safe_temperature();
       #endif
 
       idle(true);
@@ -9204,6 +9222,8 @@ inline void gcode_M503() {
    */
   inline void gcode_M600() {
 
+    ensure_safe_temperature();
+
     // Initial retract before move to filament change position
     const float retract = parser.seen('E') ? parser.value_axis_units(E_AXIS) : 0
       #if defined(PAUSE_PARK_RETRACT_LENGTH) && PAUSE_PARK_RETRACT_LENGTH > 0
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 0508bca6c..cb6eabfc3 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -1094,6 +1094,7 @@ void kill_screen(const char* lcd_msg) {
   #endif
 
   #if ENABLED(ADVANCED_PAUSE_FEATURE)
+
     void lcd_enqueue_filament_change() {
       if (!DEBUGGING(DRYRUN) && thermalManager.tooColdToExtrude(active_extruder)) {
         lcd_save_previous_screen();
@@ -1103,7 +1104,8 @@ void kill_screen(const char* lcd_msg) {
       lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_INIT);
       enqueue_and_echo_commands_P(PSTR("M600 B0"));
     }
-  #endif
+
+  #endif // ADVANCED_PAUSE_FEATURE
 
   /**
    *