mcu: Support minclock in send_with_response() requests

Enhance send_with_response() so that it supports queries with a
minclock.

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

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

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)

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']