3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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>
This commit is contained in:
Kevin O'Connor 2017-09-18 21:41:00 -04:00
parent 2089f19670
commit 13812aa1c9
4 changed files with 139 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
klippy/clocksync.py Normal file
View File

@ -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)

13
klippy/console.py
View File

@ -5,8 +5,7 @@
# #
# This file may be distributed under the terms of the GNU GPLv3 license. # This file may be distributed under the terms of the GNU GPLv3 license.
import sys, optparse, os, re, logging import sys, optparse, os, re, logging
import reactor, serialhdl, pins, util, msgproto, clocksync
import reactor, serialhdl, pins, util, msgproto
help_txt = """ help_txt = """
This is a debugging console for the Klipper micro-controller. This is a debugging console for the Klipper micro-controller.
@ -34,6 +33,7 @@ class KeyboardReader:
def __init__(self, ser, reactor): def __init__(self, ser, reactor):
self.ser = ser self.ser = ser
self.reactor = reactor self.reactor = reactor
self.clocksync = clocksync.ClockSync(self.reactor)
self.fd = sys.stdin.fileno() self.fd = sys.stdin.fileno()
util.set_nonblock(self.fd) util.set_nonblock(self.fd)
self.mcu_freq = 0 self.mcu_freq = 0
@ -52,6 +52,7 @@ class KeyboardReader:
self.output(help_txt) self.output(help_txt)
self.output("="*20 + " attempting to connect " + "="*20) self.output("="*20 + " attempting to connect " + "="*20)
self.ser.connect() self.ser.connect()
self.clocksync.connect(self.ser)
self.ser.handle_default = self.handle_default self.ser.handle_default = self.handle_default
self.mcu_freq = self.ser.msgparser.get_constant_float('CLOCK_FREQ') self.mcu_freq = self.ser.msgparser.get_constant_float('CLOCK_FREQ')
mcu = self.ser.msgparser.get_constant('MCU') mcu = self.ser.msgparser.get_constant('MCU')
@ -68,7 +69,7 @@ class KeyboardReader:
pass pass
def update_evals(self, eventtime): def update_evals(self, eventtime):
self.eval_globals['freq'] = self.mcu_freq 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): def command_PINS(self, parts):
mcu = self.ser.msgparser.get_constant('MCU') mcu = self.ser.msgparser.get_constant('MCU')
self.pins = pins.get_pin_map(mcu, parts[1]) self.pins = pins.get_pin_map(mcu, parts[1])
@ -104,7 +105,7 @@ class KeyboardReader:
self.output("Error: %s" % (str(e),)) self.output("Error: %s" % (str(e),))
return return
delay_clock = int(delay * self.mcu_freq) 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) + self.mcu_freq * .200)
for i in range(count): for i in range(count):
next_clock = msg_clock + delay_clock next_clock = msg_clock + delay_clock
@ -121,7 +122,9 @@ class KeyboardReader:
return return
self.ser.register_callback(self.handle_suppress, name, oid) self.ser.register_callback(self.handle_suppress, name, oid)
def command_STATS(self, parts): 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): def command_LIST(self, parts):
self.update_evals(self.reactor.monotonic()) self.update_evals(self.reactor.monotonic())
mp = self.ser.msgparser mp = self.ser.msgparser

31
klippy/mcu.py
View File

@ -4,7 +4,7 @@
# #
# This file may be distributed under the terms of the GNU GPLv3 license. # This file may be distributed under the terms of the GNU GPLv3 license.
import sys, os, zlib, logging, math import sys, os, zlib, logging, math
import serialhdl, pins, chelper import serialhdl, pins, chelper, clocksync
class error(Exception): class error(Exception):
pass pass
@ -205,8 +205,8 @@ class MCU_endstop:
s.note_homing_triggered() s.note_homing_triggered()
self._homing = False self._homing = False
return False return False
if (self._mcu.serial.get_clock(last_sent_time) last_clock, last_clock_time = self._mcu.get_last_clock()
> self._home_timeout_clock): if last_clock > self._home_timeout_clock:
# Timeout - disable endstop checking # Timeout - disable endstop checking
msg = self._home_cmd.encode(self._oid, 0, 0, 0) msg = self._home_cmd.encode(self._oid, 0, 0, 0)
self._mcu.send(msg, reqclock=0, cq=self._cmd_queue) self._mcu.send(msg, reqclock=0, cq=self._cmd_queue)
@ -397,7 +397,7 @@ class MCU_adc:
, self._oid) , self._oid)
def _handle_analog_in_state(self, params): def _handle_analog_in_state(self, params):
last_value = params['value'] * self._inv_max_adc 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_clock = next_clock - self._report_clock
last_read_time = self._mcu.clock_to_print_time(last_read_clock) last_read_time = self._mcu.clock_to_print_time(last_read_clock)
if self._callback is not None: if self._callback is not None:
@ -406,8 +406,9 @@ class MCU_adc:
class MCU: class MCU:
error = error error = error
COMM_TIMEOUT = 3.5 COMM_TIMEOUT = 3.5
def __init__(self, printer, config): def __init__(self, printer, config, clocksync):
self._printer = printer self._printer = printer
self._clocksync = clocksync
# Serial port # Serial port
self._serialport = config.get('serial', '/dev/ttyS0') self._serialport = config.get('serial', '/dev/ttyS0')
if self._serialport.startswith("/dev/rpmsg_"): if self._serialport.startswith("/dev/rpmsg_"):
@ -488,6 +489,7 @@ class MCU:
# Try toggling usb power # Try toggling usb power
self._check_restart("enable power") self._check_restart("enable power")
self.serial.connect() self.serial.connect()
self._clocksync.connect(self.serial)
self._printer.reactor.update_timer( self._printer.reactor.update_timer(
self._timeout_timer, self.monotonic() + self.COMM_TIMEOUT) self._timeout_timer, self.monotonic() + self.COMM_TIMEOUT)
self._mcu_freq = self.serial.msgparser.get_constant_float('CLOCK_FREQ') self._mcu_freq = self.serial.msgparser.get_constant_float('CLOCK_FREQ')
@ -510,13 +512,14 @@ class MCU:
dict_data = dfile.read() dict_data = dfile.read()
dfile.close() dfile.close()
self.serial.connect_file(outfile, dict_data) self.serial.connect_file(outfile, dict_data)
self._clocksync.connect_file(self.serial, pace)
# Handle pacing # Handle pacing
if not pace: if not pace:
def dummy_estimated_print_time(eventtime): def dummy_estimated_print_time(eventtime):
return 0. return 0.
self.estimated_print_time = dummy_estimated_print_time self.estimated_print_time = dummy_estimated_print_time
def timeout_handler(self, eventtime): 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 timeout = last_clock_time + self.COMM_TIMEOUT
if eventtime < timeout: if eventtime < timeout:
return timeout return timeout
@ -530,9 +533,10 @@ class MCU:
self._ffi_lib.steppersync_free(self._steppersync) self._ffi_lib.steppersync_free(self._steppersync)
self._steppersync = None self._steppersync = None
def stats(self, eventtime): def stats(self, eventtime):
return "%s mcu_awake=%.03f mcu_task_avg=%.06f mcu_task_stddev=%.06f" % ( msg = "mcu_awake=%.03f mcu_task_avg=%.06f mcu_task_stddev=%.06f" % (
self.serial.stats(eventtime),
self._mcu_tick_awake, self._mcu_tick_avg, self._mcu_tick_stddev) 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): def force_shutdown(self):
self.send(self._emergency_stop_cmd.encode()) self.send(self._emergency_stop_cmd.encode())
def microcontroller_restart(self): def microcontroller_restart(self):
@ -545,7 +549,7 @@ class MCU:
chelper.run_hub_ctrl(1) chelper.run_hub_ctrl(1)
return return
if self._restart_method == 'command': 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() eventtime = reactor.monotonic()
if ((self._reset_cmd is None and self._config_reset_cmd is None) if ((self._reset_cmd is None and self._config_reset_cmd is None)
or eventtime > last_clock_time + self.COMM_TIMEOUT): or eventtime > last_clock_time + self.COMM_TIMEOUT):
@ -691,13 +695,15 @@ class MCU:
def clock_to_print_time(self, clock): def clock_to_print_time(self, clock):
return clock / self._mcu_freq return clock / self._mcu_freq
def estimated_print_time(self, eventtime): 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): def get_mcu_freq(self):
return self._mcu_freq return self._mcu_freq
def seconds_to_clock(self, time): def seconds_to_clock(self, time):
return int(time * self._mcu_freq) return int(time * self._mcu_freq)
def get_last_clock(self): 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): def get_max_stepper_error(self):
return self._max_stepper_error return self._max_stepper_error
# Move command queuing # Move command queuing
@ -741,4 +747,5 @@ def error_help(msg):
return "" return ""
def add_printer_objects(printer, config): 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))

102
klippy/serialhdl.py
View File

@ -1,6 +1,6 @@
# Serial port management for firmware communication # 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. # This file may be distributed under the terms of the GNU GPLv3 license.
import logging, threading import logging, threading
@ -8,8 +8,6 @@ import serial
import msgproto, chelper, util import msgproto, chelper, util
MAX_CLOCK_DRIFT = 0.000100
class error(Exception): class error(Exception):
pass pass
@ -27,19 +25,12 @@ class SerialReader:
self.serialqueue = None self.serialqueue = None
self.default_cmd_queue = self.alloc_command_queue() self.default_cmd_queue = self.alloc_command_queue()
self.stats_buf = self.ffi_main.new('char[4096]') 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 # Threading
self.lock = threading.Lock() self.lock = threading.Lock()
self.background_thread = None self.background_thread = None
# Message handlers # Message handlers
self.status_timer = self.reactor.register_timer(self._status_event)
self.status_cmd = None
handlers = { handlers = {
'#unknown': self.handle_unknown, '#unknown': self.handle_unknown, '#output': self.handle_output,
'#output': self.handle_output, 'status': self.handle_status,
'shutdown': self.handle_output, 'is_shutdown': self.handle_output 'shutdown': self.handle_output, 'is_shutdown': self.handle_output
} }
self.handlers = { (k, None): v for k, v in handlers.items() } 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 baud_adjust = self.BITS_PER_BYTE / mcu_baud
self.ffi_lib.serialqueue_set_baud_adjust( self.ffi_lib.serialqueue_set_baud_adjust(
self.serialqueue, 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): def connect_file(self, debugoutput, dictionary, pace=False):
self.ser = debugoutput self.ser = debugoutput
self.msgparser.process_identify(dictionary, decompress=False) 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.serialqueue = self.ffi_lib.serialqueue_alloc(self.ser.fileno(), 1)
self.min_freq = self.max_freq = est_freq def set_clock_est(self, freq, last_time, last_clock):
self.last_clock = 0
self.last_clock_time = self.reactor.monotonic()
self.ffi_lib.serialqueue_set_clock_est( self.ffi_lib.serialqueue_set_clock_est(
self.serialqueue, self.min_freq, self.last_clock_time self.serialqueue, freq, last_time, last_clock)
, self.last_clock)
def disconnect(self): def disconnect(self):
if self.serialqueue is not None: if self.serialqueue is not None:
self.ffi_lib.serialqueue_exit(self.serialqueue) self.ffi_lib.serialqueue_exit(self.serialqueue)
@ -141,15 +114,9 @@ class SerialReader:
def stats(self, eventtime): def stats(self, eventtime):
if self.serialqueue is None: if self.serialqueue is None:
return "" return ""
sqstats = self.ffi_lib.serialqueue_get_stats( self.ffi_lib.serialqueue_get_stats(
self.serialqueue, self.stats_buf, len(self.stats_buf)) self.serialqueue, self.stats_buf, len(self.stats_buf))
sqstats = self.ffi_main.string(self.stats_buf) return 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
# Serial response callbacks # Serial response callbacks
def register_callback(self, callback, name, oid=None): def register_callback(self, callback, name, oid=None):
with self.lock: with self.lock:
@ -157,23 +124,6 @@ class SerialReader:
def unregister_callback(self, name, oid=None): def unregister_callback(self, name, oid=None):
with self.lock: with self.lock:
del self.handlers[name, oid] 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 # Command sending
def send(self, cmd, minclock=0, reqclock=0, cq=None): def send(self, cmd, minclock=0, reqclock=0, cq=None):
if cq is None: if cq is None:
@ -212,46 +162,6 @@ class SerialReader:
logging.info("Receive: %d %f %f %d: %s" % ( logging.info("Receive: %d %f %f %d: %s" % (
i, msg.receive_time, msg.sent_time, msg.len, ', '.join(cmds))) i, msg.receive_time, msg.sent_time, msg.len, ', '.join(cmds)))
# Default message handlers # 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): def handle_unknown(self, params):
logging.warn("Unknown message type %d: %s" % ( logging.warn("Unknown message type %d: %s" % (
params['#msgid'], repr(params['#msg']))) params['#msgid'], repr(params['#msg'])))