diff --git a/klippy/mcu.py b/klippy/mcu.py index dc0011c9..bd7267ad 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -9,37 +9,143 @@ import serialhdl, msgproto, pins, chelper, clocksync class error(Exception): pass -class MCU_endstop: - RETRY_QUERY = 1.000 - def __init__(self, mcu, pin_params): +class MCU_trsync: + REASON_ENDSTOP_HIT = 1 + REASON_COMMS_TIMEOUT = 2 + REASON_HOST_REQUEST = 3 + REASON_PAST_END_TIME = 4 + def __init__(self, mcu, trdispatch): self._mcu = mcu - self._steppers = [] - self._pin = pin_params['pin'] - self._pullup = pin_params['pullup'] - self._invert = pin_params['invert'] + self._trdispatch = trdispatch self._reactor = mcu.get_printer().get_reactor() - self._oid = self._home_cmd = self._query_cmd = None - self._ts_oid = self._trsync_start_cmd = self._trsync_trigger_cmd = None - self._mcu.register_config_callback(self._build_config) - self._min_query_time = self._last_sent_time = self._end_home_time = 0. - self._trigger_completion = self._home_completion = None + self._steppers = [] + self._trdispatch_mcu = None + self._oid = mcu.create_oid() + self._cmd_queue = mcu.alloc_command_queue() + self._trsync_start_cmd = self._trsync_set_timeout_cmd = None + self._trsync_trigger_cmd = self._trsync_query_cmd = None + self._stepper_stop_cmd = None + self._trigger_completion = None + self._home_end_clock = None + mcu.register_config_callback(self._build_config) + printer = mcu.get_printer() + printer.register_event_handler("klippy:shutdown", self._shutdown) def get_mcu(self): return self._mcu + def get_oid(self): + return self._oid + def get_command_queue(self): + return self._cmd_queue def add_stepper(self, stepper): - if stepper.get_mcu() is not self._mcu: - raise pins.error("Endstop and stepper must be on the same mcu") if stepper in self._steppers: return self._steppers.append(stepper) def get_steppers(self): return list(self._steppers) def _build_config(self): + mcu = self._mcu # Setup config - self._ts_oid = self._mcu.create_oid() - self._mcu.add_config_cmd("config_trsync oid=%d" % (self._ts_oid,)) - self._mcu.add_config_cmd("trsync_trigger oid=%d reason=0" - % (self._ts_oid,), on_restart=True) + mcu.add_config_cmd("config_trsync oid=%d" % (self._oid,)) + mcu.add_config_cmd( + "trsync_start oid=%d report_clock=0 report_ticks=0 expire_reason=0" + % (self._oid,), on_restart=True) + # Lookup commands + self._trsync_start_cmd = mcu.lookup_command( + "trsync_start oid=%c report_clock=%u report_ticks=%u" + " expire_reason=%c", cq=self._cmd_queue) + self._trsync_set_timeout_cmd = mcu.lookup_command( + "trsync_set_timeout oid=%c clock=%u", cq=self._cmd_queue) + self._trsync_trigger_cmd = mcu.lookup_command( + "trsync_trigger oid=%c reason=%c", cq=self._cmd_queue) + self._trsync_query_cmd = mcu.lookup_query_command( + "trsync_trigger oid=%c reason=%c", + "trsync_state oid=%c can_trigger=%c trigger_reason=%c clock=%u", + oid=self._oid, cq=self._cmd_queue) + self._stepper_stop_cmd = mcu.lookup_command( + "stepper_stop_on_trigger oid=%c trsync_oid=%c", cq=self._cmd_queue) + # Create trdispatch_mcu object + set_timeout_tag = mcu.lookup_command_tag( + "trsync_set_timeout oid=%c clock=%u") + trigger_tag = mcu.lookup_command_tag("trsync_trigger oid=%c reason=%c") + state_tag = mcu.lookup_command_tag( + "trsync_state oid=%c can_trigger=%c trigger_reason=%c clock=%u") + ffi_main, ffi_lib = chelper.get_ffi() + self._trdispatch_mcu = ffi_main.gc(ffi_lib.trdispatch_mcu_alloc( + self._trdispatch, mcu._serial.serialqueue, # XXX + self._cmd_queue, self._oid, set_timeout_tag, trigger_tag, + state_tag), ffi_lib.free) + def _shutdown(self): + tc = self._trigger_completion + if tc is not None: + self._trigger_completion = None + tc.complete(False) + def _handle_trsync_state(self, params): + if not params['can_trigger']: + tc = self._trigger_completion + if tc is not None: + self._trigger_completion = None + self._reactor.async_complete(tc, True) + elif self._home_end_clock is not None: + clock = self._mcu.clock32_to_clock64(params['clock']) + if clock >= self._home_end_clock: + self._home_end_clock = None + self._trsync_trigger_cmd.send([self._oid, + self.REASON_PAST_END_TIME]) + def start(self, print_time, trigger_completion, expire_timeout): + self._trigger_completion = trigger_completion + self._home_end_clock = None + clock = self._mcu.print_time_to_clock(print_time) + expire_ticks = self._mcu.seconds_to_clock(expire_timeout) + expire_clock = clock + expire_ticks + report_ticks = self._mcu.seconds_to_clock(expire_timeout * .4) + min_extend_ticks = self._mcu.seconds_to_clock(expire_timeout * .4 * .8) + ffi_main, ffi_lib = chelper.get_ffi() + ffi_lib.trdispatch_mcu_setup(self._trdispatch_mcu, clock, expire_clock, + expire_ticks, min_extend_ticks) + self._mcu.register_response(self._handle_trsync_state, + "trsync_state", self._oid) + self._trsync_start_cmd.send([self._oid, clock, report_ticks, + self.REASON_COMMS_TIMEOUT]) + for s in self._steppers: + self._stepper_stop_cmd.send([s.get_oid(), self._oid]) + self._trsync_set_timeout_cmd.send([self._oid, expire_clock]) + def set_home_end_time(self, home_end_time): + self._home_end_clock = self._mcu.print_time_to_clock(home_end_time) + def stop(self): + self._mcu.register_response(None, "trsync_state", self._oid) + self._trigger_completion = None + if self._mcu.is_fileoutput(): + return self.REASON_ENDSTOP_HIT + params = self._trsync_query_cmd.send([self._oid, + self.REASON_HOST_REQUEST]) + for s in self._steppers: + s.note_homing_end(did_trigger=True) # XXX + return params['trigger_reason'] + +class MCU_endstop: + RETRY_QUERY = 1.000 + def __init__(self, mcu, pin_params): + self._mcu = mcu + self._pin = pin_params['pin'] + self._pullup = pin_params['pullup'] + self._invert = pin_params['invert'] self._oid = self._mcu.create_oid() + self._home_cmd = self._query_cmd = None + self._mcu.register_config_callback(self._build_config) + self._trigger_completion = None + ffi_main, ffi_lib = chelper.get_ffi() + self._trdispatch = ffi_main.gc(ffi_lib.trdispatch_alloc(), ffi_lib.free) + self._trsync = MCU_trsync(mcu, self._trdispatch) + def get_mcu(self): + return self._mcu + def add_stepper(self, stepper): + if stepper.get_mcu() is not self._mcu: + raise pins.error("Endstop and stepper must be on the same mcu") + self._trsync.add_stepper(stepper) + def get_steppers(self): + return self._trsync.get_steppers() + def _build_config(self): + # Setup config self._mcu.add_config_cmd("config_endstop oid=%d pin=%s pull_up=%d" % (self._oid, self._pin, self._pullup)) self._mcu.add_config_cmd( @@ -47,14 +153,7 @@ class MCU_endstop: " rest_ticks=0 pin_value=0 trsync_oid=0 trigger_reason=0" % (self._oid,), on_restart=True) # Lookup commands - cmd_queue = self._mcu.alloc_command_queue() - self._trsync_start_cmd = self._mcu.lookup_command( - "trsync_start oid=%c report_clock=%u report_ticks=%u" - " expire_reason=%c", cq=cmd_queue) - self._trsync_trigger_cmd = self._mcu.lookup_command( - "trsync_trigger oid=%c reason=%c", cq=cmd_queue) - self._stepper_stop_cmd = self._mcu.lookup_command( - "stepper_stop_on_trigger oid=%c trsync_oid=%c", cq=cmd_queue) + cmd_queue = self._trsync.get_command_queue() self._home_cmd = self._mcu.lookup_command( "endstop_home oid=%c clock=%u sample_ticks=%u sample_count=%c" " rest_ticks=%u pin_value=%c trsync_oid=%c trigger_reason=%c", @@ -67,55 +166,28 @@ class MCU_endstop: triggered=True): clock = self._mcu.print_time_to_clock(print_time) rest_ticks = self._mcu.print_time_to_clock(print_time+rest_time) - clock - self._min_query_time = self._reactor.monotonic() - self._last_sent_time = 0. - self._home_end_time = self._reactor.NEVER - self._trigger_completion = self._reactor.completion() - self._mcu.register_response(self._handle_trsync_state, - "trsync_state", self._ts_oid) - report_ticks = self._mcu.seconds_to_clock(0.100) - self._trsync_start_cmd.send([self._ts_oid, clock, report_ticks, 0], - reqclock=clock) - for s in self._steppers: - self._stepper_stop_cmd.send([s.get_oid(), self._ts_oid]) + reactor = self._mcu.get_printer().get_reactor() + self._trigger_completion = reactor.completion() + etrsync = self._trsync + etrsync.start(print_time, self._trigger_completion, .250) + ffi_main, ffi_lib = chelper.get_ffi() + ffi_lib.trdispatch_start(self._trdispatch, etrsync.REASON_HOST_REQUEST) self._home_cmd.send( [self._oid, clock, self._mcu.seconds_to_clock(sample_time), sample_count, rest_ticks, triggered ^ self._invert, - self._ts_oid, 0], reqclock=clock) - self._home_completion = self._reactor.register_callback( - self._home_retry) + etrsync.get_oid(), etrsync.REASON_ENDSTOP_HIT], reqclock=clock) return self._trigger_completion - def _handle_trsync_state(self, params): - logging.debug("trsync_state %s", params) - if params['#sent_time'] >= self._min_query_time: - if params['can_trigger']: - self._last_sent_time = params['#sent_time'] - else: - self._min_query_time = self._reactor.NEVER - self._reactor.async_complete(self._trigger_completion, True) - def _home_retry(self, eventtime): - if self._mcu.is_fileoutput(): - return True - while 1: - did_trigger = self._trigger_completion.wait(eventtime + 0.100) - if did_trigger is not None: - # Homing completed successfully - return True - # Check for timeout - last = self._mcu.estimated_print_time(self._last_sent_time) - if last > self._home_end_time or self._mcu.is_shutdown(): - return False def home_wait(self, home_end_time): - self._home_end_time = home_end_time - did_trigger = self._home_completion.wait() - self._trsync_trigger_cmd.send([self._ts_oid, 0]) - self._mcu.register_response(None, "trsync_state", self._ts_oid) + etrsync = self._trsync + etrsync.set_home_end_time(home_end_time) + if self._mcu.is_fileoutput(): + self._trigger_completion.complete(True) + self._trigger_completion.wait() self._home_cmd.send([self._oid, 0, 0, 0, 0, 0, 0, 0]) - for s in self._steppers: - s.note_homing_end(did_trigger=did_trigger) - if not self._trigger_completion.test(): - self._trigger_completion.complete(False) - return did_trigger + ffi_main, ffi_lib = chelper.get_ffi() + ffi_lib.trdispatch_stop(self._trdispatch) + res = etrsync.stop() + return res == etrsync.REASON_ENDSTOP_HIT def query_endstop(self, print_time): clock = self._mcu.print_time_to_clock(print_time) if self._mcu.is_fileoutput():