homing: Implement homing via new toolhead "drip" movement

Rework the low-level implementation of homing movement.  The existing
mechanism buffers all homing movement into the micro-controller prior
to starting the home.  Replace with a system that buffers all movement
into the host look-ahead buffer and then "drip feed" those moves to
the micro-controllers.  Then clear the host look-ahead buffer when all
endstops trigger.

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

klippy/extras/bltouch.py

@@ -157,10 +157,11 @@ class BLTouchEndstopWrapper:
             if s.get_mcu_position() == mcu_pos:
                 raise homing.EndstopError("BLTouch failed to deploy")
         self.mcu_endstop.home_finalize()
-    def home_start(self, print_time, sample_time, sample_count, rest_time):
+    def home_start(self, print_time, sample_time, sample_count, rest_time,
+                   notify=None):
         rest_time = min(rest_time, ENDSTOP_REST_TIME)
-        self.mcu_endstop.home_start(
+        self.mcu_endstop.home_start(print_time, sample_time, sample_count,
-            print_time, sample_time, sample_count, rest_time)
+                                    rest_time, notify=notify)
     def get_position_endstop(self):
         return self.position_endstop
     cmd_BLTOUCH_DEBUG_help = "Send a command to the bltouch for debugging"

klippy/homing.py

@@ -16,6 +16,7 @@ class Homing:
         self.toolhead = printer.lookup_object('toolhead')
         self.changed_axes = []
         self.verify_retract = True
+        self.endstops_pending = -1
     def set_no_verify_retract(self):
         self.verify_retract = False
     def set_axes(self, axes):
@@ -39,6 +40,10 @@ class Homing:
         dist_ticks = adjusted_freq * mcu_stepper.get_step_dist()
         ticks_per_step = math.ceil(dist_ticks / speed)
         return dist_ticks / ticks_per_step
+    def _endstop_notify(self):
+        self.endstops_pending -= 1
+        if not self.endstops_pending:
+            self.toolhead.signal_drip_mode_end()
     def homing_move(self, movepos, endstops, speed, dwell_t=0.,
                     probe_pos=False, verify_movement=False):
         # Notify endstops of upcoming home
@@ -50,22 +55,22 @@ class Homing:
         print_time = self.toolhead.get_last_move_time()
         start_mcu_pos = [(s, name, s.get_mcu_position())
                          for es, name in endstops for s in es.get_steppers()]
+        self.endstops_pending = len(endstops)
         for mcu_endstop, name in endstops:
             min_step_dist = min([s.get_step_dist()
                                  for s in mcu_endstop.get_steppers()])
             mcu_endstop.home_start(
                 print_time, ENDSTOP_SAMPLE_TIME, ENDSTOP_SAMPLE_COUNT,
-                min_step_dist / speed)
+                min_step_dist / speed, notify=self._endstop_notify)
-        self.toolhead.dwell(HOMING_START_DELAY, check_stall=False)
+        self.toolhead.dwell(HOMING_START_DELAY)
         # Issue move
         error = None
         try:
-            self.toolhead.move(movepos, speed)
+            self.toolhead.drip_move(movepos, speed)
         except CommandError as e:
             error = "Error during homing move: %s" % (str(e),)
         # Wait for endstops to trigger
         move_end_print_time = self.toolhead.get_last_move_time()
-        self.toolhead.reset_print_time(print_time)
         for mcu_endstop, name in endstops:
             try:
                 mcu_endstop.home_wait(move_end_print_time)

klippy/mcu.py

@@ -1,6 +1,6 @@
 # Interface to Klipper micro-controller code
 #
-# Copyright (C) 2016-2018  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2016-2019  Kevin O'Connor <kevin@koconnor.net>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import sys, os, zlib, logging, math
@@ -149,8 +149,9 @@ class MCU_endstop:
         self._oid = self._home_cmd = self._query_cmd = None
         self._mcu.register_config_callback(self._build_config)
         self._min_query_time = self._last_sent_time = 0.
-        self._next_query_print_time = 0.
+        self._next_query_print_time = self._end_home_time = 0.
-        self._completion = None
+        self._trigger_completion = self._home_completion = None
+        self._trigger_notify = None
     def get_mcu(self):
         return self._mcu
     def add_stepper(self, stepper):
@@ -182,21 +183,24 @@ class MCU_endstop:
     def home_prepare(self):
         pass
     def home_start(self, print_time, sample_time, sample_count, rest_time,
-                   triggered=True):
+                   triggered=True, notify=None):
         clock = self._mcu.print_time_to_clock(print_time)
         rest_ticks = int(rest_time * self._mcu.get_adjusted_freq())
+        self._trigger_notify = notify
         self._next_query_print_time = print_time + self.RETRY_QUERY
         self._min_query_time = self._reactor.monotonic()
         self._last_sent_time = 0.
-        self._completion = self._reactor.completion()
+        self._home_end_time = self._reactor.NEVER
+        self._trigger_completion = self._reactor.completion()
+        self._home_completion = self._reactor.completion()
         self._mcu.register_response(self._handle_end_stop_state,
                                     "end_stop_state", self._oid)
         self._home_cmd.send(
             [self._oid, clock, self._mcu.seconds_to_clock(sample_time),
              sample_count, rest_ticks, triggered ^ self._invert],
             reqclock=clock)
-        for s in self._steppers:
+        self._home_completion = self._reactor.register_callback(
-            s.note_homing_start(clock)
+            self._home_retry)
     def _handle_end_stop_state(self, params):
         logging.debug("end_stop_state %s", params)
         if params['#sent_time'] >= self._min_query_time:
@@ -204,36 +208,36 @@ class MCU_endstop:
                 self._last_sent_time = params['#sent_time']
             else:
                 self._min_query_time = self._reactor.NEVER
-                self._reactor.async_complete(self._completion, params)
+                self._reactor.async_complete(self._trigger_completion, params)
-    def home_wait(self, home_end_time):
+    def _home_retry(self, eventtime):
         if self._mcu.is_fileoutput():
-            self._completion.complete({})
+            return True
-        curtime = self._reactor.monotonic()
         while 1:
-            params = self._completion.wait(curtime + 0.100)
+            params = self._trigger_completion.wait(eventtime + 0.100)
             if params is not None:
                 # Homing completed successfully
-                self._mcu.register_response(None, "end_stop_state", self._oid)
+                if self._trigger_notify is not None:
-                for s in self._steppers:
+                    self._trigger_notify()
-                    s.note_homing_end(did_trigger=True)
+                return True
-                return
             # Check for timeout
             last = self._mcu.estimated_print_time(self._last_sent_time)
-            if last > home_end_time:
+            if last > self._home_end_time or self._mcu.is_shutdown():
-                # Timeout - disable endstop checking
+                return False
-                self._mcu.register_response(None, "end_stop_state", self._oid)
-                for s in self._steppers:
-                    s.note_homing_end()
-                self._home_cmd.send([self._oid, 0, 0, 0, 0, 0])
-                raise self.TimeoutError("Timeout during endstop homing")
             # Check for resend
-            curtime = self._reactor.monotonic()
+            eventtime = self._reactor.monotonic()
-            est_print_time = self._mcu.estimated_print_time(curtime)
+            est_print_time = self._mcu.estimated_print_time(eventtime)
             if est_print_time >= self._next_query_print_time:
                 self._next_query_print_time = est_print_time + self.RETRY_QUERY
                 self._query_cmd.send([self._oid])
-            if self._mcu.is_shutdown():
+    def home_wait(self, home_end_time):
-                raise error("MCU is shutdown")
+        self._home_end_time = home_end_time
+        did_trigger = self._home_completion.wait()
+        self._mcu.register_response(None, "end_stop_state", self._oid)
+        self._home_cmd.send([self._oid, 0, 0, 0, 0, 0])
+        for s in self._steppers:
+            s.note_homing_end(did_trigger=did_trigger)
+        if not did_trigger:
+            raise self.TimeoutError("Timeout during endstop homing")
     def home_finalize(self):
         pass
     def query_endstop(self, print_time):

klippy/toolhead.py

@@ -194,6 +194,11 @@ class MoveQueue:
 
 STALL_TIME = 0.100
 
+DRIP_SEGMENT_TIME = 0.050
+DRIP_TIME = 0.150
+class DripModeEndSignal(Exception):
+    pass
+
 # Main code to track events (and their timing) on the printer toolhead
 class ToolHead:
     def __init__(self, config):
@@ -238,6 +243,7 @@ class ToolHead:
         self.last_print_start_time = 0.
         self.idle_flush_print_time = 0.
         self.print_stall = 0
+        self.drip_completion = None
         # Setup iterative solver
         ffi_main, ffi_lib = chelper.get_ffi()
         self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free)
@@ -282,7 +288,19 @@ class ToolHead:
             self.printer.send_event("toolhead:sync_print_time",
                                     curtime, est_print_time, self.print_time)
     def get_next_move_time(self):
-        if self.special_queuing_state:
+        if not self.special_queuing_state:
+            return self.print_time
+        if self.special_queuing_state == "Drip":
+            # In "Drip" state - wait until ready to send next move
+            while 1:
+                if self.drip_completion.test():
+                    raise DripModeEndSignal()
+                curtime = self.reactor.monotonic()
+                est_print_time = self.mcu.estimated_print_time(curtime)
+                wait_time = self.print_time - est_print_time - DRIP_TIME
+                if wait_time <= 0. or self.mcu.is_fileoutput():
+                    return self.print_time
+                self.drip_completion.wait(curtime + wait_time)
         # Transition from "Flushed"/"Priming" state to main state
         self.special_queuing_state = ""
         self.need_check_stall = -1.
@@ -405,6 +423,42 @@ class ToolHead:
         self.commanded_pos[3] = extrude_pos
     def get_extruder(self):
         return self.extruder
+    def drip_move(self, newpos, speed):
+        # Validate move
+        move = Move(self, self.commanded_pos, newpos, speed)
+        if move.axes_d[3]:
+            raise homing.CommandError("Invalid drip move")
+        if not move.move_d or not move.is_kinematic_move:
+            return
+        self.kin.check_move(move)
+        speed = math.sqrt(move.max_cruise_v2)
+        # Transition to "Flushed" state and then to "Drip" state
+        self._full_flush()
+        self.special_queuing_state = "Drip"
+        self.need_check_stall = self.reactor.NEVER
+        self.reactor.update_timer(self.flush_timer, self.reactor.NEVER)
+        self.move_queue.set_flush_time(self.reactor.NEVER)
+        self.drip_completion = self.reactor.completion()
+        # Split move into many tiny moves and queue them
+        num_moves = max(1, int(math.ceil(move.min_move_t / DRIP_SEGMENT_TIME)))
+        inv_num_moves = 1. / float(num_moves)
+        submove_d = [d * inv_num_moves for d in move.axes_d]
+        prev_pos = move.start_pos
+        for i in range(num_moves-1):
+            next_pos = [p + d for p, d in zip(prev_pos, submove_d)]
+            self.move_queue.add_move(Move(self, prev_pos, next_pos, speed))
+            prev_pos = next_pos
+        self.move_queue.add_move(Move(self, prev_pos, move.end_pos, speed))
+        # Transmit moves
+        self._calc_print_time()
+        try:
+            self.move_queue.flush()
+        except DripModeEndSignal as e:
+            self.move_queue.reset()
+        # Return to "Flushed" state
+        self._full_flush()
+    def signal_drip_mode_end(self):
+        self.drip_completion.complete(True)
     # Misc commands
     def stats(self, eventtime):
         for m in self.all_mcus: