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.
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

31
klippy/mcu.py
View File

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

102
klippy/serialhdl.py
View File

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