toolhead: Force a firmware shutdown on an unhandled exception

Check for unhandled exceptions and force the MCU to shutdown in that
case.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

klippy/gcode.py

@@ -101,7 +101,8 @@ class GCodeParser:
                 handler(params)
             except:
                 logging.exception("Exception in command handler")
-                self.respond('echo:Internal error on command:"%s"' % (cmd,))
+                self.toolhead.force_shutdown()
+                self.respond('Error: Internal error on command:"%s"' % (cmd,))
             # Check if machine can process next command or must stall input
             if self.busy_state is not None:
                 break
@@ -142,7 +143,13 @@ class GCodeParser:
         self.busy_state = busy_handler
         self.reactor.update_timer(self.busy_timer, self.reactor.NOW)
     def busy_handler(self, eventtime):
+        try:
             busy = self.busy_state.check_busy(eventtime)
+        except:
+            logging.exception("Exception in busy handler")
+            self.toolhead.force_shutdown()
+            self.respond('Error: Internal error in busy handler')
+            busy = False
         if busy:
             self.toolhead.reset_motor_off_time(eventtime)
             return eventtime + self.RETRY_TIME

klippy/mcu.py

@@ -252,6 +252,7 @@ class MCU:
         self.is_shutdown = False
         self.output_file_mode = False
         # Config building
+        self._emergency_stop_cmd = None
         self._num_oids = 0
         self._config_cmds = []
         self._config_crc = None
@@ -328,6 +329,8 @@ class MCU:
         if err:
             stats += " step_errors=%d" % (err,)
         return stats
+    def force_shutdown(self):
+        self.send(self._emergency_stop_cmd.encode())
     # Configuration phase
     def _add_custom(self):
         data = self._config.get('custom', '')
@@ -340,6 +343,7 @@ class MCU:
                 continue
             self.add_config_cmd(line)
     def build_config(self):
+        self._emergency_stop_cmd = self.lookup_command("emergency_stop")
         # Build config commands
         self._add_custom()
         self._config_cmds.insert(0, "allocate_oids count=%d" % (
@@ -504,6 +508,8 @@ class DummyMCU:
         pass
     def stats(self, eventtime):
         return ""
+    def force_shutdown(self):
+        pass
     def build_config(self):
         pass
     def create_stepper(self, step_pin, dir_pin, min_stop_interval, max_error):

klippy/toolhead.py

@@ -85,6 +85,9 @@ class MoveQueue:
         self.queue = []
         self.prev_junction_max = 0.
         self.junction_flush = 0.
+    def reset(self):
+        del self.queue[:]
+        self.prev_junction_max = self.junction_flush = 0.
     def flush(self, lazy=False):
         can_flush = not lazy
         flush_count = len(self.queue)
@@ -186,6 +189,7 @@ class ToolHead:
             eventtime, self.print_time)
         return buffer_time > self.buffer_time_high
     def flush_handler(self, eventtime):
+        try:
             if not self.print_time:
                 self.move_queue.flush()
                 if not self.print_time:
@@ -206,6 +210,9 @@ class ToolHead:
                 return self.reactor.NOW
             self.reset_print_time()
             return self.motor_off_time
+        except:
+            logging.exception("Exception in flush_handler")
+            self.force_shutdown()
     def stats(self, eventtime):
         buffer_time = 0.
         if self.print_time:
@@ -273,3 +280,6 @@ class ToolHead:
         self.extruder.motor_off(last_move_time)
         self.dwell(STALL_TIME)
         logging.debug('; Max time of %f' % (last_move_time,))
+    def force_shutdown(self):
+        self.printer.mcu.force_shutdown()
+        self.move_queue.reset()