From 13812aa1c9f76c869f70de0e1934750c70914898 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 18 Sep 2017 21:41:00 -0400 Subject: 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 --- klippy/serialhdl.py | 102 ++++------------------------------------------------ 1 file changed, 6 insertions(+), 96 deletions(-) (limited to 'klippy/serialhdl.py') 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 +# Copyright (C) 2016,2017 Kevin O'Connor # # 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']))) -- cgit v1.2.3-70-g09d2