-
Notifications
You must be signed in to change notification settings - Fork 4
/
timesync.c
132 lines (106 loc) · 3.95 KB
/
timesync.c
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
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
#include <strings.h>
#include <assert.h>
#include <stdlib.h>
#include <math.h>
#include "timesync.h"
/** An implementation of "A Passive Solution to the Sensor
* Synchronization Problem" by Edwin Olson, 2010.
*
* We assume that f() is a linear model with slope 'rate_error'. We
* compute only the causal version here.
**/
timesync_t *
timesync_create(double device_ticks_per_second, int64_t device_ticks_wrap,
double rate_error, double reset_time)
{
timesync_t *ts = calloc (1, sizeof(*ts));
ts->p_ticks = -1; // negative means "no data"
ts->device_ticks_per_second = device_ticks_per_second;
ts->device_ticks_wrap = device_ticks_wrap;
ts->rate_error = rate_error;
ts->reset_time = reset_time;
return ts;
}
void
timesync_destroy (timesync_t *ts)
{
free (ts);
}
/** This function must be called prior to a call to get_host_utime for
* the same device_ticks.
**/
void
timesync_update (timesync_t *ts, int64_t host_utime, int64_t device_ticks_wrapping)
{
assert(device_ticks_wrapping >= 0);
// check for wrap-around
if (device_ticks_wrapping < ts->last_device_ticks_wrapping) {
// wrap around has occurred.
ts->device_ticks_offset += ts->device_ticks_wrap;
}
ts->last_device_ticks_wrapping = device_ticks_wrapping;
int64_t device_ticks = ts->device_ticks_offset + device_ticks_wrapping;
/* We can rewrite the update equations from the paper:
pi - qi >= p - q - f(pi-p)
as
pi - p >= qi - q - f(pi-p)
dp >= dq - f(dp)
This form is superior, because we only need to be able to
accurately represent changes in time for any single clock. This
avoids precision problems that can occur in the simple
formulation when the two clocks have very different magnitudes.
**/
int64_t pi_ticks = device_ticks;
int64_t qi_ticks = host_utime;
double dp = (pi_ticks - ts->p_ticks) / ts->device_ticks_per_second;
double dq = (qi_ticks - ts->q_ticks) / 1.0E6;
ts->last_sync_error = fabs (dp - dq);
if (ts->p_ticks == -1 || ts->last_sync_error >= ts->reset_time) {
// force resynchronize
ts->p_ticks = pi_ticks;
ts->q_ticks = qi_ticks;
// used for diagnostics/debugging
ts->resync_count++;
return;
}
if (dp >= dq - fabs (ts->rate_error * dp)) {
ts->p_ticks = pi_ticks;
ts->q_ticks = qi_ticks;
}
}
int64_t
timesync_get_host_utime (timesync_t *ts, int64_t device_ticks_wrapping)
{
// timesync_update must called first.
assert (ts->p_ticks != -1);
assert (device_ticks_wrapping >= 0);
// pick the most recent device_ticks that could match device_ticks_wrapping.
// There are two candidates:
// 1) device_ticks_offset + device_ticks_wrapping
// and
// 2) device_ticks_offset + device_ticks_wrapping - device_ticks_wrap
//
// We pick the most recent (and not future) one. Note that 1) can
// be in the future.
//
// Now, we require that _update() be called on any host/device
// pairs before this function is called, so all we need to do is
// determine whether this device_ticks_wrapping is part of the
// same epoch.
int64_t device_ticks;
if (device_ticks_wrapping <= ts->last_device_ticks_wrapping) {
// They're asking about an earlier timestamp from this epoch.
device_ticks = ts->device_ticks_offset + device_ticks_wrapping;
}
else {
// They're asking about a timestamp from the previous
// epoch. (They could be asking about multiple epochs ago, but
// we don't attempt to resolve that ambiguity.)
device_ticks = ts->device_ticks_offset + device_ticks_wrapping - ts->device_ticks_wrap;
}
int64_t pi_ticks = device_ticks;
// dp: time in seconds
double dp = (pi_ticks - ts->p_ticks) / ts->device_ticks_per_second;
// return units in usecs.
return ((int64_t) (dp*1.0E6)) + ts->q_ticks + ((int64_t) 1.0E6*fabs(ts->rate_error * dp));
}