mcu: Enable trdispatch infrastructure during homing
Separate out trsync handling from endstop handling in mcu.py code. Enable the low-level trdispatch C code. This is in preparation for multi-mcu homing. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor
parent
e1f7748e1a
commit
983951443c
210
klippy/mcu.py
210
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():
|
||||
|
|
|
|||
Loading…
Reference in New Issue