Add an emergency-command parser to MarlinSerial (supporting M108)

Add an emergency-command parser to MarlinSerial's RX interrupt.

The parser tries to find and execute M108,M112,M410 before the commands disappear in the RX-buffer.

To avoid false positives for M117, comments and commands followed by filenames (M23, M28, M30, M32, M33) are filtered.

This enables Marlin to receive and react on the Emergency command at all times - regardless of whether the buffers are full or not. It remains to convince hosts to send the commands. To inform the hosts about the new feature a new entry in the M115-report was made. "`EMERGENCY_CODES:M112,M108,M410;`".

The parser is fast. It only ever needs two switch decisions and one assignment of the new state for every character.

One problem remains. If the host has sent an incomplete line before sending an emergency command the emergency command could be omitted when the parser is in `state_IGNORE`.
In that case the host should send "\ncommand\n"

Also introduces M108 to break the waiting for the heaters in M109, M190 and M303.

Rename `cancel_heatup` to `wait_for_heatup` to better see the purpose.

Marlin/Conditionals.h

@@ -284,6 +284,12 @@
     #define HardwareSerial_h // trick to disable the standard HWserial
   #endif
 
+  #if ENABLED(EMERGENCY_PARSER)
+    #define EMERGENCY_PARSER_CAPABILITIES " EMERGENCY_CODES:M108,M112,M410"
+  #else
+    #define EMERGENCY_PARSER_CAPABILITIES ""
+  #endif
+
   #include "Arduino.h"
 
   /**

Marlin/Configuration_adv.h

@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/Marlin.h

@@ -288,6 +288,7 @@ extern float sw_endstop_min[3]; // axis[n].sw_endstop_min
 extern float sw_endstop_max[3]; // axis[n].sw_endstop_max
 extern bool axis_known_position[3]; // axis[n].is_known
 extern bool axis_homed[3]; // axis[n].is_homed
+extern bool wait_for_heatup;
 
 // GCode support for external objects
 bool code_seen(char);

Marlin/MarlinSerial.cpp

@@ -30,6 +30,7 @@
 
 #include "Marlin.h"
 #include "MarlinSerial.h"
+#include "stepper.h"
 
 #ifndef USBCON
 // this next line disables the entire HardwareSerial.cpp,
@@ -54,6 +55,10 @@ FORCE_INLINE void store_char(unsigned char c) {
       rx_buffer.head = i;
     }
   CRITICAL_SECTION_END;
+
+  #if ENABLED(EMERGENCY_PARSER)
+    emergency_parser(c);
+  #endif
 }
 
 
@@ -310,3 +315,156 @@ MarlinSerial customizedSerial;
 #if defined(USBCON) && ENABLED(BLUETOOTH)
   HardwareSerial bluetoothSerial;
 #endif
+
+#if ENABLED(EMERGENCY_PARSER)
+
+  // Currently looking for: M108, M112, M410
+  // If you alter the parser please don't forget to update the capabilities in Conditionals.h
+
+  void emergency_parser(unsigned char c) {
+
+    enum e_parser_state {
+      state_RESET,
+      state_M,
+      state_M1,
+      state_M10,
+      state_M11,
+      state_M2,
+      state_M3,
+      state_M4,
+      state_M41,
+      state_IGNORE // to '\n'
+    };
+
+    static e_parser_state state = state_RESET;
+
+    switch (state) {
+      case state_RESET:
+        switch (c) {
+          case 'M':
+            state = state_M;
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M:
+        switch (c) {
+          case '1':
+            state = state_M1;
+            break;
+          case '2':
+            state = state_M2;
+            break;
+          case '3':
+            state = state_M3;
+            break;
+          case '4':
+            state = state_M4;
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M1:
+        switch (c) {
+          case '0':
+            state = state_M10;
+            break;
+          case '1':
+            state = state_M11;
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M2:
+        switch (c) {
+          case '3': // M23
+          case '8': // M28
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M3:
+        switch (c) {
+          case '0': // M30
+          case '2': // M32
+          case '3': // M33
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M10:
+        switch (c) {
+          case '8': // M108
+            { state = state_RESET; wait_for_heatup = false; }
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M11:
+        switch (c) {
+          case '2': // M112
+            state = state_RESET; kill(PSTR(MSG_KILLED));
+            break;
+          case '7': // M117
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M4:
+        switch (c) {
+          case '1':
+            state = state_M41;
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M41:
+        switch (c) {
+          case '0':
+            { state = state_RESET; stepper.quick_stop(); }
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_IGNORE:
+        if (c == '\n') state = state_RESET;
+      break;
+
+      default:
+        state = state_RESET;
+    }
+  }
+#endif

Marlin/MarlinSerial.h

@@ -101,6 +101,11 @@ struct ring_buffer {
   extern ring_buffer rx_buffer;
 #endif
 
+#if ENABLED(EMERGENCY_PARSER)
+  #include "language.h"
+  void emergency_parser(unsigned char c);
+#endif
+
 class MarlinSerial { //: public Stream
 
   public:
@@ -141,6 +146,10 @@ class MarlinSerial { //: public Stream
             rx_buffer.head = i;
           }
         CRITICAL_SECTION_END;
+
+        #if ENABLED(EMERGENCY_PARSER)
+          emergency_parser(c);
+        #endif
       }
     }
 

Marlin/Marlin_main.cpp

@@ -160,7 +160,7 @@
  * M105 - Read current temp
  * M106 - Fan on
  * M107 - Fan off
- * M108 - Cancel heatup and wait for the hotend and bed, this G-code is asynchronously handled in the get_serial_commands() parser
+ * M108 - Stop the waiting for heaters in M109, M190, M303. Does not affect the target temperature.
  * M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
  *        Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
  *        IF AUTOTEMP is enabled, S<mintemp> B<maxtemp> F<factor>. Exit autotemp by any M109 without F
@@ -1105,9 +1105,12 @@ inline void get_serial_commands() {
         }
       }
 
+      #if DISABLED(EMERGENCY_PARSER)
         // If command was e-stop process now
-      if (strcmp(command, "M112") == 0) kill(PSTR(MSG_KILLED));
         if (strcmp(command, "M108") == 0) wait_for_heatup = false;
+        if (strcmp(command, "M112") == 0) kill(PSTR(MSG_KILLED));
+        if (strcmp(command, "M410") == 0) stepper.quick_stop();
+      #endif
 
       #if defined(NO_TIMEOUTS) && NO_TIMEOUTS > 0
         last_command_time = ms;
@@ -4533,10 +4536,14 @@ inline void gcode_M105() {
 
 #endif // FAN_COUNT > 0
 
-/**
- * M108: Cancel heatup and wait for the hotend and bed, this G-code is asynchronously handled in the get_serial_commands() parser
+#if DISABLED(EMERGENCY_PARSER)
+
+  /**
+   * M108: Stop the waiting for heaters in M109, M190, M303. Does not affect the target temperature.
    */
-inline void gcode_M108() { wait_for_heatup = false; }
+  inline void gcode_M108() { wait_for_heatup = false; }
+
+#endif
 
 /**
  * M109: Sxxx Wait for extruder(s) to reach temperature. Waits only when heating.
@@ -4811,7 +4818,9 @@ inline void gcode_M111() {
 /**
  * M112: Emergency Stop
  */
-inline void gcode_M112() { kill(PSTR(MSG_KILLED)); }
+#if DISABLED(EMERGENCY_PARSER)
+  inline void gcode_M112() { kill(PSTR(MSG_KILLED)); }
+#endif
 
 #if ENABLED(HOST_KEEPALIVE_FEATURE)
 
@@ -5991,13 +6000,15 @@ inline void gcode_M400() { stepper.synchronize(); }
  * This will stop the carriages mid-move, so most likely they
  * will be out of sync with the stepper position after this.
  */
-inline void gcode_M410() {
+
+#if DISABLED(EMERGENCY_PARSER)
+  inline void gcode_M410() {
     stepper.quick_stop();
     #if DISABLED(DELTA) && DISABLED(SCARA)
       set_current_position_from_planner();
     #endif
-}
-
+  }
+#endif
 
 #if ENABLED(MESH_BED_LEVELING)
 
@@ -6953,9 +6964,11 @@ void process_next_command() {
         gcode_M111();
         break;
 
+      #if DISABLED(EMERGENCY_PARSER)
         case 112: // M112: Emergency Stop
           gcode_M112();
           break;
+      #endif
 
       #if ENABLED(HOST_KEEPALIVE_FEATURE)
 
@@ -6974,9 +6987,11 @@ void process_next_command() {
         KEEPALIVE_STATE(NOT_BUSY);
         return; // "ok" already printed
 
+      #if DISABLED(EMERGENCY_PARSER)
         case 108:
           gcode_M108();
           break;
+      #endif
 
       case 109: // M109: Wait for temperature
         gcode_M109();
@@ -7261,9 +7276,11 @@ void process_next_command() {
           break;
       #endif // ENABLED(FILAMENT_WIDTH_SENSOR)
 
+      #if DISABLED(EMERGENCY_PARSER)
         case 410: // M410 quickstop - Abort all the planned moves.
           gcode_M410();
           break;
+      #endif
 
       #if ENABLED(MESH_BED_LEVELING)
         case 420: // M420 Enable/Disable Mesh Bed Leveling

Marlin/SanityCheck.h

@@ -579,6 +579,13 @@
 #endif
 
 /**
+ * emergency-command parser
+ */
+#if ENABLED(EMERGENCY_PARSER) && ENABLED(USBCON)
+  #error "EMERGENCY_PARSER does not work on boards with AT90USB processors (USBCON)."
+#endif
+
+ /**
  * Warnings for old configurations
  */
 #if WATCH_TEMP_PERIOD > 500

Marlin/example_configurations/Cartesio/Configuration_adv.h

@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/Felix/Configuration_adv.h

@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/Hephestos/Configuration_adv.h

@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/Hephestos_2/Configuration_adv.h

@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/K8200/Configuration_adv.h

@@ -526,6 +526,12 @@ const unsigned int dropsegments = 2; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/K8400/Configuration_adv.h

@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 26
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/RigidBot/Configuration_adv.h

@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 8
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/SCARA/Configuration_adv.h

@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/TAZ4/Configuration_adv.h

@@ -528,6 +528,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/WITBOX/Configuration_adv.h

@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/delta/biv2.5/Configuration_adv.h

@@ -522,6 +522,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/delta/generic/Configuration_adv.h

@@ -522,6 +522,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h

@@ -521,6 +521,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h

@@ -526,6 +526,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h

@@ -522,6 +522,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/makibox/Configuration_adv.h

@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h

@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.

Marlin/language.h

@@ -128,7 +128,7 @@
 #define MSG_INVALID_EXTRUDER                "Invalid extruder"
 #define MSG_INVALID_SOLENOID                "Invalid solenoid"
 #define MSG_ERR_NO_THERMISTORS              "No thermistors - no temperature"
-#define MSG_M115_REPORT                     "FIRMWARE_NAME:Marlin " DETAILED_BUILD_VERSION " SOURCE_CODE_URL:" SOURCE_CODE_URL " PROTOCOL_VERSION:" PROTOCOL_VERSION " MACHINE_TYPE:" MACHINE_NAME " EXTRUDER_COUNT:" STRINGIFY(EXTRUDERS) " UUID:" MACHINE_UUID "\n"
+#define MSG_M115_REPORT                     "FIRMWARE_NAME:Marlin " DETAILED_BUILD_VERSION " SOURCE_CODE_URL:" SOURCE_CODE_URL " PROTOCOL_VERSION:" PROTOCOL_VERSION " MACHINE_TYPE:" MACHINE_NAME " EXTRUDER_COUNT:" STRINGIFY(EXTRUDERS) " UUID:" MACHINE_UUID EMERGENCY_PARSER_CAPABILITIES "\n"
 #define MSG_COUNT_X                         " Count X: "
 #define MSG_COUNT_A                         " Count A: "
 #define MSG_ERR_KILLED                      "Printer halted. kill() called!"

Marlin/temperature.cpp

@@ -238,8 +238,10 @@ unsigned char Temperature::soft_pwm[HOTENDS];
       soft_pwm_bed = bias = d = (MAX_BED_POWER) / 2;
     #endif
 
+    wait_for_heatup = true;
+
     // PID Tuning loop
-    for (;;) {
+    while (wait_for_heatup) {
 
       millis_t ms = millis();
 
@@ -421,6 +423,7 @@ unsigned char Temperature::soft_pwm[HOTENDS];
       }
       lcd_update();
     }
+    if (!wait_for_heatup) disable_all_heaters();
   }
 
 #endif // HAS_PID_HEATING