diff --git a/klippy/clocksync.py b/klippy/clocksync.py index 5dffc215..ada09256 100644 --- a/klippy/clocksync.py +++ b/klippy/clocksync.py @@ -131,6 +131,9 @@ class ClockSync: def get_clock(self, eventtime): sample_time, clock, freq = self.clock_est return int(clock + (eventtime - sample_time) * freq) + def estimate_clock_systime(self, reqclock): + sample_time, clock, freq = self.clock_est + return float(reqclock - clock)/freq + sample_time def estimated_print_time(self, eventtime): return self.clock_to_print_time(self.get_clock(eventtime)) # misc commands diff --git a/klippy/mcu.py b/klippy/mcu.py index b10a9c99..15148a86 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -426,19 +426,25 @@ class MCU_adc: # Wrapper around command sending class CommandWrapper: - def __init__(self, mcu, serial, cmd, cmd_queue): + def __init__(self, mcu, serial, clocksync, cmd, cmd_queue): self._mcu = mcu self._serial = serial + self._clocksync = clocksync self._cmd = cmd self._cmd_queue = cmd_queue def send(self, data=(), minclock=0, reqclock=0): cmd = self._cmd.encode(data) self._serial.raw_send(cmd, minclock, reqclock, self._cmd_queue) - def send_with_response(self, data=(), response=None, response_oid=None): + def send_with_response(self, data=(), response=None, response_oid=None, + minclock=0): + minsystime = 0. + if minclock: + minsystime = self._clocksync.estimate_clock_systime(minclock) cmd = self._cmd.encode(data) try: - src = serialhdl.SerialRetryCommand(self._serial, cmd, - response, response_oid) + src = serialhdl.SerialRetryCommand( + self._serial, [cmd], self._cmd_queue, response, response_oid, + minclock=minclock, minsystime=minsystime) return src.get_response() except serialhdl.error as e: raise error(str(e)) @@ -710,7 +716,7 @@ class MCU: if cq is None: cq = self._serial.get_default_command_queue() cmd = self._serial.get_msgparser().lookup_command(msgformat) - return CommandWrapper(self, self._serial, cmd, cq) + return CommandWrapper(self, self._serial, self._clocksync, cmd, cq) def try_lookup_command(self, msgformat): try: return self.lookup_command(msgformat) diff --git a/klippy/serialhdl.py b/klippy/serialhdl.py index 99fd5e94..1b4f1e13 100644 --- a/klippy/serialhdl.py +++ b/klippy/serialhdl.py @@ -152,7 +152,7 @@ class SerialReader: self.raw_send(cmd, minclock, reqclock, self.default_cmd_queue) def send_with_response(self, msg, response): cmd = self.msgparser.create_command(msg) - src = SerialRetryCommand(self, cmd, response) + src = SerialRetryCommand(self, [cmd], self.default_cmd_queue, response) return src.get_response() def alloc_command_queue(self): return self.ffi_main.gc(self.ffi_lib.serialqueue_alloc_commandqueue(), @@ -199,17 +199,22 @@ class SerialReader: class SerialRetryCommand: TIMEOUT_TIME = 5.0 RETRY_TIME = 0.500 - def __init__(self, serial, cmd, name, oid=None): + def __init__(self, serial, cmds, cmd_queue, name, oid=None, + minclock=0, minsystime=0.): self.serial = serial - self.cmd = cmd + self.cmds = cmds + self.cmd_queue = cmd_queue self.name = name self.oid = oid + self.minclock = minclock self.response = None reactor = self.serial.reactor self.mutex = reactor.mutex(is_locked=True) self.min_query_time = self.serial.reactor.monotonic() + self.first_query_time = max(self.min_query_time, minsystime) self.serial.register_response(self.handle_callback, self.name, self.oid) - retry_time = self.send_event(self.min_query_time) + self.send_event(self.min_query_time) + retry_time = self.first_query_time + self.RETRY_TIME self.send_timer = reactor.register_timer(self.send_event, retry_time) def unregister(self): self.serial.unregister_response(self.name, self.oid) @@ -217,12 +222,14 @@ class SerialRetryCommand: def send_event(self, eventtime): if self.response is not None: return self.serial.reactor.NEVER - if eventtime > self.min_query_time + self.TIMEOUT_TIME: + if eventtime > self.first_query_time + self.TIMEOUT_TIME: self.unregister() if self.response is None: self.mutex.unlock() return self.serial.reactor.NEVER - self.serial.raw_send(self.cmd, 0, 0, self.serial.default_cmd_queue) + for cmd in self.cmds: + self.serial.raw_send(cmd, self.minclock, self.minclock, + self.cmd_queue) return eventtime + self.RETRY_TIME def handle_callback(self, params): last_sent_time = params['#sent_time']