serialhdl: Rework mcu clock synchronization

The existing clock synchronization code has two flaws: intermittent
transmission latency during get_status requests can cause the
estimated clock to be too low, and the estimated clock calculation did
not take into account possible clock drift between samples.  The
former could potentially lead to "Timer too close" errors and the
latter could potentially lead to "Move queue empty" errors.

Rework the code to avoid the above problems.  It's not necessary to
estimate the micro-controller clock as an excellent estimate is
reported by the micro-controller (via the CLOCK_FREQ constant).
Account for a small drift from the reported value, and check on each
sample if the drift exceeds the expected limits.  With a good starting
estimated clock, only the offset needs to be calculated.  Use previous
offsets (and the estimated clock) in calculation of new offsets to
avoid intermittent latency from badly skewing the results.  Finally,
add an additional time offset of one millisecond to account for any
minor inaccuracies.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-06-27 20:01:11 -04:00
parent c957e4ba86
commit c7d0358c41
1 changed files with 68 additions and 52 deletions

View File

@ -8,6 +8,8 @@ import serial
import msgproto, chelper, util import msgproto, chelper, util
MAX_CLOCK_DRIFT = 0.000100
class error(Exception): class error(Exception):
pass pass
@ -26,9 +28,9 @@ class SerialReader:
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 # MCU time/clock tracking
self.last_ack_time = self.last_ack_rtt_time = 0. self.last_clock = 0
self.last_ack_clock = self.last_ack_rtt_clock = 0 self.last_clock_time = self.last_clock_time_min = 0.
self.est_clock = 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
@ -101,34 +103,31 @@ 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)
# Enable periodic get_status timer # Load initial last_clock/last_clock_time
get_status = msgparser.lookup_command('get_status')
self.status_cmd = get_status.encode()
self.reactor.update_timer(self.status_timer, self.reactor.NOW)
# Load initial last_ack_clock/last_ack_time
uptime_msg = msgparser.create_command('get_uptime') uptime_msg = msgparser.create_command('get_uptime')
params = self.send_with_response(uptime_msg, 'uptime') params = self.send_with_response(uptime_msg, 'uptime')
self.last_ack_clock = (params['high'] << 32) | params['clock'] self.last_clock = (params['high'] << 32) | params['clock']
self.last_ack_time = params['#receive_time'] self.last_clock_time = params['#receive_time']
# Make sure est_clock is calculated self.last_clock_time_min = params['#sent_time']
starttime = eventtime = self.reactor.monotonic() clock_freq = msgparser.get_constant_float('CLOCK_FREQ')
while not self.est_clock: self.min_freq = clock_freq * (1. - MAX_CLOCK_DRIFT)
if eventtime > starttime + 5.: self.max_freq = clock_freq * (1. + MAX_CLOCK_DRIFT)
raise error("timeout on est_clock calculation") # Enable periodic get_status timer
eventtime = self.reactor.pause(eventtime + 0.010) 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_clock = 1000000000000. est_freq = 1000000000000.
if pace: if pace:
est_clock = float(self.msgparser.config['CLOCK_FREQ']) 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.est_clock = est_clock self.min_freq = self.max_freq = est_freq
self.last_ack_time = self.reactor.monotonic() self.last_clock = 0
self.last_ack_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.est_clock, self.last_ack_time self.serialqueue, self.min_freq, self.last_clock_time
, self.last_ack_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)
@ -145,8 +144,8 @@ class SerialReader:
sqstats = self.ffi_lib.serialqueue_get_stats( sqstats = 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) sqstats = self.ffi_main.string(self.stats_buf)
tstats = " est_clock=%.3f last_ack_time=%.3f last_ack_clock=%d" % ( tstats = " last_clock=%d last_clock_time=%.3f" % (
self.est_clock, self.last_ack_time, self.last_ack_clock) self.last_clock, self.last_clock_time)
return sqstats + tstats return sqstats + tstats
def _status_event(self, eventtime): def _status_event(self, eventtime):
self.send(self.status_cmd) self.send(self.status_cmd)
@ -161,18 +160,18 @@ class SerialReader:
# Clock tracking # Clock tracking
def get_clock(self, eventtime): def get_clock(self, eventtime):
with self.lock: with self.lock:
return int(self.last_ack_clock return int(self.last_clock
+ (eventtime - self.last_ack_time) * self.est_clock) + (eventtime - self.last_clock_time) * self.min_freq)
def translate_clock(self, raw_clock): def translate_clock(self, raw_clock):
with self.lock: with self.lock:
last_ack_clock = self.last_ack_clock last_clock = self.last_clock
clock_diff = (last_ack_clock - raw_clock) & 0xffffffff clock_diff = (last_clock - raw_clock) & 0xffffffff
if clock_diff & 0x80000000: if clock_diff & 0x80000000:
return last_ack_clock + 0x100000000 - clock_diff return last_clock + 0x100000000 - clock_diff
return last_ack_clock - clock_diff return last_clock - clock_diff
def get_last_clock(self): def get_last_clock(self):
with self.lock: with self.lock:
return self.last_ack_clock, self.last_ack_time 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:
@ -210,28 +209,45 @@ class SerialReader:
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): 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: with self.lock:
# Update last_ack_time / last_ack_clock # Extend clock to 64bit
ack_clock = (self.last_ack_clock & ~0xffffffff) | params['clock'] clock = (self.last_clock & ~0xffffffff) | clock
if ack_clock < self.last_ack_clock: if clock < self.last_clock:
ack_clock += 0x100000000 clock += 0x100000000
sent_time = params['#sent_time'] # Calculate expected send time from clock and previous estimates
self.last_ack_time = receive_time = params['#receive_time'] clock_delta = clock - self.last_clock
self.last_ack_clock = ack_clock min_send_time = (self.last_clock_time_min
# Update est_clock (if applicable) + clock_delta / self.max_freq)
if receive_time > self.last_ack_rtt_time + 1. and sent_time: max_send_time = self.last_clock_time + clock_delta / self.min_freq
if self.last_ack_rtt_time: # Calculate intersection of times
timedelta = receive_time - self.last_ack_rtt_time min_time = max(min_send_time, sent_time)
clockdelta = ack_clock - self.last_ack_rtt_clock max_time = min(max_send_time, receive_time)
estclock = clockdelta / timedelta if min_time > max_time:
if estclock > self.est_clock and self.est_clock: # No intersection - clock drift must be greater than expected
self.est_clock = (self.est_clock * 63. + estclock) / 64. new_min_freq, new_max_freq = self.min_freq, self.max_freq
else: if min_send_time > receive_time:
self.est_clock = estclock new_max_freq = (
self.last_ack_rtt_time = sent_time clock_delta / (receive_time - self.last_clock_time_min))
self.last_ack_rtt_clock = ack_clock 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.ffi_lib.serialqueue_set_clock_est(
self.serialqueue, self.est_clock, receive_time, ack_clock) 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'])))