diff options
| -rw-r--r-- | klippy/clocksync.py | 106 | ||||
| -rwxr-xr-x | klippy/console.py | 13 | ||||
| -rw-r--r-- | klippy/mcu.py | 31 | ||||
| -rw-r--r-- | klippy/serialhdl.py | 102 |
4 files changed, 139 insertions, 113 deletions
diff --git a/klippy/clocksync.py b/klippy/clocksync.py new file mode 100644 index 00000000..416648cb --- /dev/null +++ b/klippy/clocksync.py @@ -0,0 +1,106 @@ +# Micro-controller clock synchronization +# +# 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 + +MAX_CLOCK_DRIFT = 0.000100 + +class ClockSync: + def __init__(self, reactor): + self.reactor = reactor + self.serial = None + self.lock = threading.Lock() + self.status_timer = self.reactor.register_timer(self._status_event) + self.status_cmd = None + self.last_clock = 0 + self.last_clock_time = self.last_clock_time_min = 0. + self.min_freq = self.max_freq = 0. + def connect(self, serial): + self.serial = serial + # Load initial last_clock/last_clock_time + msgparser = serial.msgparser + uptime_msg = msgparser.create_command('get_uptime') + params = serial.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 + serial.register_callback(self._handle_status, 'status') + self.status_cmd = msgparser.create_command('get_status') + self.reactor.update_timer(self.status_timer, self.reactor.NOW) + def connect_file(self, serial, pace=False): + self.serial = serial + est_freq = 1000000000000. + if pace: + est_freq = float(self.msgparser.config['CLOCK_FREQ']) + self.min_freq = self.max_freq = est_freq + self.last_clock = 0 + self.last_clock_time = self.reactor.monotonic() + serial.set_clock_est( + self.min_freq, self.last_clock_time, self.last_clock) + def stats(self, eventtime): + return "last_clock=%d last_clock_time=%.3f" % ( + self.last_clock, self.last_clock_time) + 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 + def _status_event(self, eventtime): + self.serial.send(self.status_cmd) + return eventtime + 1.0 + 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.serial.set_clock_est(self.min_freq, max_time + 0.001, clock) diff --git a/klippy/console.py b/klippy/console.py index fccfee0b..c862ba9e 100755 --- a/klippy/console.py +++ b/klippy/console.py @@ -5,8 +5,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import sys, optparse, os, re, logging - -import reactor, serialhdl, pins, util, msgproto +import reactor, serialhdl, pins, util, msgproto, clocksync help_txt = """ This is a debugging console for the Klipper micro-controller. @@ -34,6 +33,7 @@ class KeyboardReader: def __init__(self, ser, reactor): self.ser = ser self.reactor = reactor + self.clocksync = clocksync.ClockSync(self.reactor) self.fd = sys.stdin.fileno() util.set_nonblock(self.fd) self.mcu_freq = 0 @@ -52,6 +52,7 @@ class KeyboardReader: self.output(help_txt) self.output("="*20 + " attempting to connect " + "="*20) self.ser.connect() + self.clocksync.connect(self.ser) self.ser.handle_default = self.handle_default self.mcu_freq = self.ser.msgparser.get_constant_float('CLOCK_FREQ') mcu = self.ser.msgparser.get_constant('MCU') @@ -68,7 +69,7 @@ class KeyboardReader: pass def update_evals(self, eventtime): self.eval_globals['freq'] = self.mcu_freq - self.eval_globals['clock'] = self.ser.get_clock(eventtime) + self.eval_globals['clock'] = self.clocksync.get_clock(eventtime) def command_PINS(self, parts): mcu = self.ser.msgparser.get_constant('MCU') self.pins = pins.get_pin_map(mcu, parts[1]) @@ -104,7 +105,7 @@ class KeyboardReader: self.output("Error: %s" % (str(e),)) return delay_clock = int(delay * self.mcu_freq) - msg_clock = int(self.ser.get_clock(self.reactor.monotonic()) + msg_clock = int(self.clocksync.get_clock(self.reactor.monotonic()) + self.mcu_freq * .200) for i in range(count): next_clock = msg_clock + delay_clock @@ -121,7 +122,9 @@ class KeyboardReader: return self.ser.register_callback(self.handle_suppress, name, oid) def command_STATS(self, parts): - self.output(self.ser.stats(self.reactor.monotonic())) + curtime = self.reactor.monotonic() + self.output(' '.join([self.ser.stats(curtime), + self.clocksync.stats(curtime)])) def command_LIST(self, parts): self.update_evals(self.reactor.monotonic()) mp = self.ser.msgparser diff --git a/klippy/mcu.py b/klippy/mcu.py index 054577c0..d045c29f 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -4,7 +4,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import sys, os, zlib, logging, math -import serialhdl, pins, chelper +import serialhdl, pins, chelper, clocksync class error(Exception): pass @@ -205,8 +205,8 @@ class MCU_endstop: s.note_homing_triggered() self._homing = False return False - if (self._mcu.serial.get_clock(last_sent_time) - > self._home_timeout_clock): + last_clock, last_clock_time = self._mcu.get_last_clock() + if last_clock > self._home_timeout_clock: # Timeout - disable endstop checking msg = self._home_cmd.encode(self._oid, 0, 0, 0) self._mcu.send(msg, reqclock=0, cq=self._cmd_queue) @@ -397,7 +397,7 @@ class MCU_adc: , self._oid) def _handle_analog_in_state(self, params): last_value = params['value'] * self._inv_max_adc - next_clock = self._mcu.serial.translate_clock(params['next_clock']) + next_clock = self._mcu.translate_clock(params['next_clock']) last_read_clock = next_clock - self._report_clock last_read_time = self._mcu.clock_to_print_time(last_read_clock) if self._callback is not None: @@ -406,8 +406,9 @@ class MCU_adc: class MCU: error = error COMM_TIMEOUT = 3.5 - def __init__(self, printer, config): + def __init__(self, printer, config, clocksync): self._printer = printer + self._clocksync = clocksync # Serial port self._serialport = config.get('serial', '/dev/ttyS0') if self._serialport.startswith("/dev/rpmsg_"): @@ -488,6 +489,7 @@ class MCU: # Try toggling usb power self._check_restart("enable power") self.serial.connect() + self._clocksync.connect(self.serial) self._printer.reactor.update_timer( self._timeout_timer, self.monotonic() + self.COMM_TIMEOUT) self._mcu_freq = self.serial.msgparser.get_constant_float('CLOCK_FREQ') @@ -510,13 +512,14 @@ class MCU: dict_data = dfile.read() dfile.close() self.serial.connect_file(outfile, dict_data) + self._clocksync.connect_file(self.serial, pace) # Handle pacing if not pace: def dummy_estimated_print_time(eventtime): return 0. self.estimated_print_time = dummy_estimated_print_time def timeout_handler(self, eventtime): - last_clock, last_clock_time = self.serial.get_last_clock() + last_clock, last_clock_time = self.get_last_clock() timeout = last_clock_time + self.COMM_TIMEOUT if eventtime < timeout: return timeout @@ -530,9 +533,10 @@ class MCU: self._ffi_lib.steppersync_free(self._steppersync) self._steppersync = None def stats(self, eventtime): - return "%s mcu_awake=%.03f mcu_task_avg=%.06f mcu_task_stddev=%.06f" % ( - self.serial.stats(eventtime), + msg = "mcu_awake=%.03f mcu_task_avg=%.06f mcu_task_stddev=%.06f" % ( self._mcu_tick_awake, self._mcu_tick_avg, self._mcu_tick_stddev) + return ' '.join([self.serial.stats(eventtime), + self._clocksync.stats(eventtime), msg]) def force_shutdown(self): self.send(self._emergency_stop_cmd.encode()) def microcontroller_restart(self): @@ -545,7 +549,7 @@ class MCU: chelper.run_hub_ctrl(1) return if self._restart_method == 'command': - last_clock, last_clock_time = self.serial.get_last_clock() + last_clock, last_clock_time = self.get_last_clock() eventtime = reactor.monotonic() if ((self._reset_cmd is None and self._config_reset_cmd is None) or eventtime > last_clock_time + self.COMM_TIMEOUT): @@ -691,13 +695,15 @@ class MCU: def clock_to_print_time(self, clock): return clock / self._mcu_freq def estimated_print_time(self, eventtime): - return self.clock_to_print_time(self.serial.get_clock(eventtime)) + return self.clock_to_print_time(self._clocksync.get_clock(eventtime)) def get_mcu_freq(self): return self._mcu_freq def seconds_to_clock(self, time): return int(time * self._mcu_freq) def get_last_clock(self): - return self.serial.get_last_clock() + return self._clocksync.get_last_clock() + def translate_clock(self, clock): + return self._clocksync.translate_clock(clock) def get_max_stepper_error(self): return self._max_stepper_error # Move command queuing @@ -741,4 +747,5 @@ def error_help(msg): return "" def add_printer_objects(printer, config): - printer.add_object('mcu', MCU(printer, config.getsection('mcu'))) + mainsync = clocksync.ClockSync(printer.reactor) + printer.add_object('mcu', MCU(printer, config.getsection('mcu'), mainsync)) 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']))) |