homing: Check for timeout during homing operation

Should a homing move complete without hitting the endstop, then
disable motors, disable the endstop checking, and report the error to
the user.

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

3 files changed, 46 insertions, 15 deletions

diff --git a/klippy/gcode.py b/klippy/gcode.py
index 5262676a..88797077 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -151,6 +151,9 @@ class GCodeParser:
     def busy_handler(self, eventtime):
         try:
             busy = self.busy_state.check_busy(eventtime)
+        except homing.EndstopError, e:
+            self.respond("Error: %s" % (e,))
+            busy = False
         except:
             logging.exception("Exception in busy handler")
             self.toolhead.force_shutdown()
diff --git a/klippy/homing.py b/klippy/homing.py
index fb0f4ca7..1cdd2543 100644
--- a/klippy/homing.py
+++ b/klippy/homing.py
@@ -3,8 +3,8 @@
 # Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
-
 import logging
+import mcu
 
 class Homing:
     def __init__(self, toolhead, changed_axes):
@@ -31,7 +31,11 @@ class Homing:
         self.eventtime = eventtime
         while self.states:
             first = self.states[0]
-            ret = first[0](*first[1])
+            try:
+                ret = first[0](*first[1])
+            except EndstopError, e:
+                self.toolhead.motor_off()
+                raise
             if ret:
                 return True
             self.states.pop(0)
@@ -55,17 +59,22 @@ class Homing:
         print_time = self.toolhead.get_last_move_time()
         for s in steppers:
             es = s.enable_endstop_checking(print_time, s.step_dist / speed)
-            self.endstops.append(es)
+            self.endstops.append((s.name, es))
         self.toolhead.move(self.fill_coord(movepos), speed)
+        move_end_print_time = self.toolhead.get_last_move_time()
         self.toolhead.reset_print_time()
-        for es in self.endstops:
-            es.home_finalize()
+        for name, es in self.endstops:
+            es.home_finalize(es.print_to_mcu_time(move_end_print_time))
         return False
     def do_wait_endstop(self):
         # Check if endstops have triggered
-        for es in self.endstops:
-            if es.check_busy(self.eventtime):
-                return True
+        for name, es in self.endstops:
+            try:
+                if es.check_busy(self.eventtime):
+                    return True
+            except mcu.MCUError, e:
+                raise EndstopError("Failed to home stepper %s: %s" % (
+                    name, str(e)))
         # Finished
         del self.endstops[:]
         return False
@@ -85,13 +94,16 @@ class QueryEndstops:
             if es is None:
                 continue
             self.endstops.append((stepper.name, es))
-            self.busy.append(es)
+            self.busy.append((stepper.name, es))
     def check_busy(self, eventtime):
         # Check if all endstop queries have been received
         while self.busy:
-            busy = self.busy[0].check_busy(eventtime)
-            if busy:
-                return True
+            try:
+                if self.busy[0][1].check_busy(eventtime):
+                    return True
+            except mcu.MCUError, e:
+                raise EndstopError("Failed to query endstop %s: %s" % (
+                    self.busy[0][0], str(e)))
             self.busy.pop(0)
         # All responses received - report status
         msgs = []
diff --git a/klippy/mcu.py b/klippy/mcu.py
index df1bb3dc..54cc78da 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -6,6 +6,12 @@
 import sys, zlib, logging, time, math
 import serialhdl, pins, chelper
 
+class MCUError(Exception):
+    def __init__(self, msg="MCU Error"):
+        self._msg = msg
+    def __str__(self):
+        return self._msg
+
 def parse_pin_extras(pin, can_pullup=False):
     pullup = invert = 0
     if can_pullup and pin.startswith('^'):
@@ -100,7 +106,7 @@ class MCU_endstop:
         self._query_cmd = mcu.lookup_command("end_stop_query oid=%c")
         self._homing = False
         self._min_query_time = 0.
-        self._next_query_clock = 0
+        self._next_query_clock = self._home_timeout_clock = 0
         self._mcu_freq = mcu.get_mcu_freq()
         self._retry_query_ticks = int(self._mcu_freq * self.RETRY_QUERY)
         self._last_state = {}
@@ -114,11 +120,12 @@ class MCU_endstop:
         msg = self._home_cmd.encode(
             self._oid, clock, rest_ticks, 1 ^ self._invert)
         self._mcu.send(msg, reqclock=clock, cq=self._cmd_queue)
-    def home_finalize(self):
+    def home_finalize(self, mcu_time):
         # XXX - this flushes the serial port of messages ready to be
         # sent, but doesn't flush messages if they had an unmet minclock
         self._mcu.serial.send_flush()
         self._stepper.note_stepper_stop()
+        self._home_timeout_clock = int(mcu_time * self._mcu_freq)
     def _handle_end_stop_state(self, params):
         logging.debug("end_stop_state %s" % (params,))
         self._last_state = params
@@ -126,9 +133,18 @@ class MCU_endstop:
         # Check if need to send an end_stop_query command
         if self._mcu.output_file_mode:
             return False
-        if self._last_state.get('#sent_time', -1.) >= self._min_query_time:
+        last_sent_time = self._last_state.get('#sent_time', -1.)
+        if last_sent_time >= self._min_query_time:
             if not self._homing or not self._last_state.get('homing', 0):
                 return False
+            if (self._mcu.serial.get_clock(last_sent_time)
+                > self._home_timeout_clock):
+                # Timeout - disable endstop checking
+                msg = self._home_cmd.encode(self._oid, 0, 0, 0)
+                self._mcu.send(msg, reqclock=0, cq=self._cmd_queue)
+                raise MCUError("Timeout during endstop homing")
+        if self._mcu.is_shutdown:
+            raise MCUError("MCU is shutdown")
         last_clock = self._mcu.get_last_clock()
         if last_clock >= self._next_query_clock:
             self._next_query_clock = last_clock + self._retry_query_ticks