clocksync: Move clock synchronization code into new file

Move the low-level clock synchronization code from serialhdl.py to a
new file clocksync.py.

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

1 files changed, 6 insertions, 96 deletions

diff --git a/klippy/serialhdl.py b/klippy/serialhdl.py
index 8626cf25..ed7b3274 100644
--- a/klippy/serialhdl.py
+++ b/klippy/serialhdl.py
@@ -1,6 +1,6 @@
 # Serial port management for firmware communication
 #
-# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2016,2017  Kevin O'Connor <kevin@koconnor.net>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import logging, threading
@@ -8,8 +8,6 @@ import serial
 
 import msgproto, chelper, util
 
-MAX_CLOCK_DRIFT = 0.000100
-
 class error(Exception):
     pass
 
@@ -27,19 +25,12 @@ class SerialReader:
         self.serialqueue = None
         self.default_cmd_queue = self.alloc_command_queue()
         self.stats_buf = self.ffi_main.new('char[4096]')
-        # MCU time/clock tracking
-        self.last_clock = 0
-        self.last_clock_time = self.last_clock_time_min = 0.
-        self.min_freq = self.max_freq = 0.
         # Threading
         self.lock = threading.Lock()
         self.background_thread = None
         # Message handlers
-        self.status_timer = self.reactor.register_timer(self._status_event)
-        self.status_cmd = None
         handlers = {
-            '#unknown': self.handle_unknown,
-            '#output': self.handle_output, 'status': self.handle_status,
+            '#unknown': self.handle_unknown, '#output': self.handle_output,
             'shutdown': self.handle_output, 'is_shutdown': self.handle_output
         }
         self.handlers = { (k, None): v for k, v in handlers.items() }
@@ -103,31 +94,13 @@ class SerialReader:
             baud_adjust = self.BITS_PER_BYTE / mcu_baud
             self.ffi_lib.serialqueue_set_baud_adjust(
                 self.serialqueue, baud_adjust)
-        # Load initial last_clock/last_clock_time
-        uptime_msg = msgparser.create_command('get_uptime')
-        params = self.send_with_response(uptime_msg, 'uptime')
-        self.last_clock = (params['high'] << 32) | params['clock']
-        self.last_clock_time = params['#receive_time']
-        self.last_clock_time_min = params['#sent_time']
-        clock_freq = msgparser.get_constant_float('CLOCK_FREQ')
-        self.min_freq = clock_freq * (1. - MAX_CLOCK_DRIFT)
-        self.max_freq = clock_freq * (1. + MAX_CLOCK_DRIFT)
-        # Enable periodic get_status timer
-        self.status_cmd = msgparser.create_command('get_status')
-        self.reactor.update_timer(self.status_timer, self.reactor.NOW)
     def connect_file(self, debugoutput, dictionary, pace=False):
         self.ser = debugoutput
         self.msgparser.process_identify(dictionary, decompress=False)
-        est_freq = 1000000000000.
-        if pace:
-            est_freq = float(self.msgparser.config['CLOCK_FREQ'])
         self.serialqueue = self.ffi_lib.serialqueue_alloc(self.ser.fileno(), 1)
-        self.min_freq = self.max_freq = est_freq
-        self.last_clock = 0
-        self.last_clock_time = self.reactor.monotonic()
+    def set_clock_est(self, freq, last_time, last_clock):
         self.ffi_lib.serialqueue_set_clock_est(
-            self.serialqueue, self.min_freq, self.last_clock_time
-            , self.last_clock)
+            self.serialqueue, freq, last_time, last_clock)
     def disconnect(self):
         if self.serialqueue is not None:
             self.ffi_lib.serialqueue_exit(self.serialqueue)
@@ -141,15 +114,9 @@ class SerialReader:
     def stats(self, eventtime):
         if self.serialqueue is None:
             return ""
-        sqstats = self.ffi_lib.serialqueue_get_stats(
+        self.ffi_lib.serialqueue_get_stats(
             self.serialqueue, self.stats_buf, len(self.stats_buf))
-        sqstats = self.ffi_main.string(self.stats_buf)
-        tstats = " last_clock=%d last_clock_time=%.3f" % (
-            self.last_clock, self.last_clock_time)
-        return sqstats + tstats
-    def _status_event(self, eventtime):
-        self.send(self.status_cmd)
-        return eventtime + 1.0
+        return self.ffi_main.string(self.stats_buf)
     # Serial response callbacks
     def register_callback(self, callback, name, oid=None):
         with self.lock:
@@ -157,23 +124,6 @@ class SerialReader:
     def unregister_callback(self, name, oid=None):
         with self.lock:
             del self.handlers[name, oid]
-    # Clock tracking
-    def get_clock(self, eventtime):
-        with self.lock:
-            last_clock = self.last_clock
-            last_clock_time = self.last_clock_time
-            min_freq = self.min_freq
-        return int(last_clock + (eventtime - last_clock_time) * min_freq)
-    def translate_clock(self, raw_clock):
-        with self.lock:
-            last_clock = self.last_clock
-        clock_diff = (last_clock - raw_clock) & 0xffffffff
-        if clock_diff & 0x80000000:
-            return last_clock + 0x100000000 - clock_diff
-        return last_clock - clock_diff
-    def get_last_clock(self):
-        with self.lock:
-            return self.last_clock, self.last_clock_time
     # Command sending
     def send(self, cmd, minclock=0, reqclock=0, cq=None):
         if cq is None:
@@ -212,46 +162,6 @@ class SerialReader:
             logging.info("Receive: %d %f %f %d: %s" % (
                 i, msg.receive_time, msg.sent_time, msg.len, ', '.join(cmds)))
     # Default message handlers
-    def handle_status(self, params):
-        sent_time = params['#sent_time']
-        if not sent_time:
-            return
-        receive_time = params['#receive_time']
-        clock = params['clock']
-        with self.lock:
-            # Extend clock to 64bit
-            clock = (self.last_clock & ~0xffffffff) | clock
-            if clock < self.last_clock:
-                clock += 0x100000000
-            # Calculate expected send time from clock and previous estimates
-            clock_delta = clock - self.last_clock
-            min_send_time = (self.last_clock_time_min
-                             + clock_delta / self.max_freq)
-            max_send_time = self.last_clock_time + clock_delta / self.min_freq
-            # Calculate intersection of times
-            min_time = max(min_send_time, sent_time)
-            max_time = min(max_send_time, receive_time)
-            if min_time > max_time:
-                # No intersection - clock drift must be greater than expected
-                new_min_freq, new_max_freq = self.min_freq, self.max_freq
-                if min_send_time > receive_time:
-                    new_max_freq = (
-                        clock_delta / (receive_time - self.last_clock_time_min))
-                else:
-                    new_min_freq = (
-                        clock_delta / (sent_time - self.last_clock_time))
-                logging.warning(
-                    "High clock drift! Now %.0f:%.0f was %.0f:%.0f" % (
-                        new_min_freq, new_max_freq,
-                        self.min_freq, self.max_freq))
-                self.min_freq, self.max_freq = new_min_freq, new_max_freq
-                min_time, max_time = sent_time, receive_time
-            # Update variables
-            self.last_clock = clock
-            self.last_clock_time = max_time
-            self.last_clock_time_min = min_time
-            self.ffi_lib.serialqueue_set_clock_est(
-                self.serialqueue, self.min_freq, max_time + 0.001, clock)
     def handle_unknown(self, params):
         logging.warn("Unknown message type %d: %s" % (
             params['#msgid'], repr(params['#msg'])))