mirror of
https://github.com/Klipper3d/klipper.git
synced 2025-07-24 07:04:01 -06:00
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
parent
2089f19670
commit
13812aa1c9
4 changed files with 139 additions and 113 deletions
|
|
@ -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'])))
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue