Mirror
/
klipper-dgus
mirror of https://github.com/Desuuuu/klipper.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

clocksync: Update clock synchronization code to use a linear regression 

Implement a "moving" linear regression between the reported mcu clock
and the sent_time of the get_status message that generated that
report.  Use this linear regression to make predictions on the
relationship between the system time and the mcu clock.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-09-30 01:58:14 -04:00
parent 61ee63f358
commit 776d8f9f79
1 changed files with 67 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
klippy/clocksync.py
View File

@ -3,11 +3,12 @@
# Copyright (C) 2016,2017 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, math
COMM_TIMEOUT = 3.5 COMM_TIMEOUT = 3.5
RTT_AGE = .000010 / (60. * 60.) RTT_AGE = .000010 / (60. * 60.)
TRANSMIT_EXTRA = .005 DECAY = 1. / (2. * 60.)
TRANSMIT_EXTRA = .001
class ClockSync: class ClockSync:
def __init__(self, reactor): def __init__(self, reactor):
@ -17,10 +18,15 @@ class ClockSync:
self.status_cmd = None self.status_cmd = None
self.mcu_freq = 1. self.mcu_freq = 1.
self.last_clock = 0 self.last_clock = 0
self.clock_est = (0., 0., 0.)
# Minimum round-trip-time tracking
self.min_half_rtt = 999999999.9 self.min_half_rtt = 999999999.9
self.min_half_rtt_time = 0. self.min_rtt_time = 0.
self.clock_est = self.prev_est = (0., 0, 0.) # Linear regression of mcu clock and system sent_time
self.last_clock_fast = False self.time_avg = self.time_variance = 0.
self.clock_avg = self.clock_covariance = 0.
self.prediction_variance = 0.
self.last_prediction_time = 0.
def connect(self, serial): def connect(self, serial):
self.serial = serial self.serial = serial
msgparser = serial.msgparser msgparser = serial.msgparser
@ -28,9 +34,11 @@ class ClockSync:
# Load initial clock and frequency # Load initial clock and frequency
uptime_msg = msgparser.create_command('get_uptime') uptime_msg = msgparser.create_command('get_uptime')
params = serial.send_with_response(uptime_msg, 'uptime') params = serial.send_with_response(uptime_msg, 'uptime')
self.last_clock = clock = (params['high'] << 32) | params['clock'] self.last_clock = (params['high'] << 32) | params['clock']
new_time = .5 * (params['#sent_time'] + params['#receive_time']) self.clock_avg = self.last_clock
self.clock_est = self.prev_est = (new_time, clock, self.mcu_freq) self.time_avg = params['#sent_time']
self.clock_est = (self.time_avg, self.clock_avg, self.mcu_freq)
self.prediction_variance = (.001 * self.mcu_freq)**2
# Enable periodic get_status timer # Enable periodic get_status timer
self.status_cmd = msgparser.create_command('get_status') self.status_cmd = msgparser.create_command('get_status')
for i in range(8): for i in range(8):
@ -46,15 +54,14 @@ class ClockSync:
if pace: if pace:
freq = self.mcu_freq freq = self.mcu_freq
serial.set_clock_est(freq, self.reactor.monotonic(), 0) serial.set_clock_est(freq, self.reactor.monotonic(), 0)
# mcu clock querying # MCU clock querying (status callback invoked from background thread)
def _status_event(self, eventtime): def _status_event(self, eventtime):
self.serial.send(self.status_cmd) self.serial.send(self.status_cmd)
return eventtime + 1.0 return eventtime + 1.0
def _handle_status(self, params): def _handle_status(self, params):
# Extend clock to 64bit # Extend clock to 64bit
clock32 = params['clock']
last_clock = self.last_clock last_clock = self.last_clock
clock = (last_clock & ~0xffffffff) | clock32 clock = (last_clock & ~0xffffffff) | params['clock']
if clock < last_clock: if clock < last_clock:
clock += 0x100000000 clock += 0x100000000
self.last_clock = clock self.last_clock = clock
@ -64,32 +71,50 @@ class ClockSync:
return return
receive_time = params['#receive_time'] receive_time = params['#receive_time']
half_rtt = .5 * (receive_time - sent_time) half_rtt = .5 * (receive_time - sent_time)
aged_rtt = (sent_time - self.min_half_rtt_time) * RTT_AGE aged_rtt = (sent_time - self.min_rtt_time) * RTT_AGE
if half_rtt < self.min_half_rtt + aged_rtt: if half_rtt < self.min_half_rtt + aged_rtt:
self.min_half_rtt = half_rtt self.min_half_rtt = half_rtt
self.min_half_rtt_time = sent_time self.min_rtt_time = sent_time
logging.debug("new minimum rtt=%.6f (%d)", half_rtt, self.mcu_freq) logging.debug("new minimum rtt %.3f: hrtt=%.6f freq=%d",
# Calculate expected clock range from sent/receive time sent_time, half_rtt, self.clock_est[2])
est_min_clock = self.get_clock(sent_time + self.min_half_rtt) # Compare clock to predicted clock and track prediction accuracy
est_max_clock = self.get_clock(receive_time - self.min_half_rtt) exp_clock = ((sent_time - self.time_avg) * self.clock_est[2]
if clock >= est_min_clock and clock <= est_max_clock: + self.clock_avg)
# Sample inline with expectations clock_diff2 = (clock - exp_clock)**2
if clock_diff2 > 25. * self.prediction_variance:
if clock > exp_clock and sent_time < self.last_prediction_time + 10.:
logging.debug("Ignoring clock sample %.3f:"
" freq=%d diff=%d stddev=%.3f",
sent_time, self.clock_est[2], clock - exp_clock,
math.sqrt(self.prediction_variance))
return return
# Update estimated frequency based on latest sample logging.info("Resetting prediction variance %.3f:"
if clock > est_max_clock: " freq=%d diff=%d stddev=%.3f",
clock_fast = True sent_time, self.clock_est[2], clock - exp_clock,
new_time = receive_time - self.min_half_rtt math.sqrt(self.prediction_variance))
self.prediction_variance = (.001 * self.mcu_freq)**2
else: else:
clock_fast = False self.last_prediction_time = sent_time
new_time = sent_time + self.min_half_rtt self.prediction_variance = (
if clock_fast != self.last_clock_fast: (1. - DECAY) * (self.prediction_variance + clock_diff2 * DECAY))
if sent_time > self.min_half_rtt_time: # Add clock and sent_time to linear regression
self.prev_est = self.clock_est diff_sent_time = sent_time - self.time_avg
self.last_clock_fast = clock_fast self.time_avg += DECAY * diff_sent_time
new_freq = (clock - self.prev_est[1]) / (new_time - self.prev_est[0]) self.time_variance = (1. - DECAY) * (
self.serial.set_clock_est( self.time_variance + diff_sent_time**2 * DECAY)
new_freq, new_time + self.min_half_rtt + TRANSMIT_EXTRA, clock) diff_clock = clock - self.clock_avg
self.clock_est = (new_time, clock, new_freq) self.clock_avg += DECAY * diff_clock
self.clock_covariance = (1. - DECAY) * (
self.clock_covariance + diff_sent_time * diff_clock * DECAY)
# Update prediction from linear regression
new_freq = self.clock_covariance / self.time_variance
pred_stddev = math.sqrt(self.prediction_variance)
self.serial.set_clock_est(new_freq, self.time_avg + TRANSMIT_EXTRA,
int(self.clock_avg - 3. * pred_stddev))
self.clock_est = (self.time_avg - self.min_half_rtt,
self.clock_avg + 3. * pred_stddev, new_freq)
#logging.debug("regr %.3f: freq=%.3f d=%d(%.3f)",
# sent_time, new_freq, clock - exp_clock, pred_stddev)
# clock frequency conversions # clock frequency conversions
def print_time_to_clock(self, print_time): def print_time_to_clock(self, print_time):
return int(print_time * self.mcu_freq) return int(print_time * self.mcu_freq)
@ -116,13 +141,15 @@ class ClockSync:
return print_time < last_clock_print_time + COMM_TIMEOUT return print_time < last_clock_print_time + COMM_TIMEOUT
def dump_debug(self): def dump_debug(self):
sample_time, clock, freq = self.clock_est sample_time, clock, freq = self.clock_est
prev_time, prev_clock, prev_freq = self.prev_est
return ("clocksync state: mcu_freq=%d last_clock=%d" return ("clocksync state: mcu_freq=%d last_clock=%d"
" min_half_rtt=%.6f min_half_rtt_time=%.3f last_clock_fast=%s" " clock_est=(%.3f %d %.3f) min_half_rtt=%.6f min_rtt_time=%.3f"
" clock_est=(%.3f %d %.3f) prev_est=(%.3f %d %.3f)" % ( " time_avg=%.3f(%.3f) clock_avg=%.3f(%.3f)"
self.mcu_freq, self.last_clock, self.min_half_rtt, " pred_variance=%.3f" % (
self.min_half_rtt_time, self.last_clock_fast, self.mcu_freq, self.last_clock, sample_time, clock, freq,
sample_time, clock, freq, prev_time, prev_clock, prev_freq)) self.min_half_rtt, self.min_rtt_time,
self.time_avg, self.time_variance,
self.clock_avg, self.clock_covariance,
self.prediction_variance))
def stats(self, eventtime): def stats(self, eventtime):
sample_time, clock, freq = self.clock_est sample_time, clock, freq = self.clock_est
return "freq=%d" % (freq,) return "freq=%d" % (freq,)