1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
|
# 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.queries_pending = 0
self.status_timer = self.reactor.register_timer(self._status_event)
self.status_cmd = None
self.lock = threading.Lock()
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 is_active(self, eventtime):
return self.queries_pending <= 4
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 _status_event(self, eventtime):
self.queries_pending += 1
self.serial.send(self.status_cmd)
return eventtime + 1.0
def _handle_status(self, params):
self.queries_pending = 0
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)
|
