-
Notifications
You must be signed in to change notification settings - Fork 3
/
phaselock.c
332 lines (312 loc) · 11 KB
/
phaselock.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
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
/*
* phaselock.c - Phase locking for NTP client
*
* Copyright (C) 2000, 2007 Larry Doolittle <larry@doolittle.boa.org>
* Last hack: 30 December, 2007
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License (Version 2,
* June 1991) as published by the Free Software Foundation. At the
* time of writing, that license was published by the FSF with the URL
* http://www.gnu.org/copyleft/gpl.html, and is incorporated herein by
* reference.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* Possible future improvements:
* - Subtract configurable amount from errorbar
* - Build in general run-time access to tune parameters
* - Sculpt code so it's legible, this version is out of control
* - Write documentation :-(
*/
#include <stdio.h>
#include "ntpclient.h"
double min_delay = 800.0; /* global, user-changeable, units are microseconds */
#define RING_SIZE 16
#define MAX_CORRECT 250 /* ppm change to system clock */
#define MAX_C ((MAX_CORRECT)*65536)
static struct datum {
unsigned int absolute;
double skew;
double errorbar;
int freq;
/* s.s.min and s.s.max (skews) are "corrected" to what they would
* have been if freq had been constant at its current value during
* the measurements.
*/
union {
struct { double min; double max; } s;
double ss[2];
} s;
/*
double smin;
double smax;
*/
} d_ring[RING_SIZE];
static struct _seg {
double slope;
double offset;
} maxseg[RING_SIZE+1], minseg[RING_SIZE+1];
#if 0
/* draw a line from a to c, what the offset is of that line
* where that line matches b's slope coordinate.
*/
static double interpolate(struct _seg *a, struct _seg *b, struct _seg *c)
{
double x, y;
x = (b->slope - a->slope) / (c->slope - a->slope) ;
y = a->offset + x * (c->offset - a->offset);
return y;
}
#endif
static int next_up(int i) { int r = i+1; if (r>=RING_SIZE) r=0; return r;}
static int next_dn(int i) { int r = i-1; if (r<0) r=RING_SIZE-1; return r;}
/* Looks at the line segments that start at point j, that end at
* all following points (ending at index rp). The initial point
* is on curve s0, the ending point is on curve s1. The curve choice
* (s.min vs. s.max) is based on the index in ss[]. The scan
* looks for the largest (sign=0) or smallest (sign=1) slope.
*/
static int search(int rp, int j, int s0, int s1, int sign, struct _seg *answer)
{
double dt, slope;
int n, nextj=0, cinit=1;
for (n=next_up(j); n!=next_up(rp); n=next_up(n)) {
if (0 && debug) printf("d_ring[%d].s.ss[%d]=%f d_ring[%d].s.ss[%d]=%f\n",
n, s0, d_ring[n].s.ss[s0], j, s1, d_ring[j].s.ss[s1]);
dt = d_ring[n].absolute - d_ring[j].absolute;
slope = (d_ring[n].s.ss[s0] - d_ring[j].s.ss[s1]) / dt;
if (0 && debug) printf("slope %d%d%d [%d,%d] = %f\n",
s0, s1, sign, j, n, slope);
if (cinit || (slope < answer->slope) ^ sign) {
answer->slope = slope;
answer->offset = d_ring[n].s.ss[s0] +
slope*(d_ring[rp].absolute - d_ring[n].absolute);
cinit = 0;
nextj = n;
}
}
return nextj;
}
/* Pseudo-class for finding consistent frequency shift */
#define MIN_INIT 20
static struct _polygon {
double l_min;
double r_min;
} df;
static void polygon_reset(void)
{
df.l_min = MIN_INIT;
df.r_min = MIN_INIT;
}
static double find_df(int *flag)
{
if (df.l_min == 0.0) {
if (df.r_min == 0.0) {
return 0.0; /* every point was OK */
} else {
return -df.r_min;
}
} else {
if (df.r_min == 0.0) {
return df.l_min;
} else {
if (flag) *flag=1;
return 0.0; /* some points on each side,
* or no data at all */
}
}
}
/* Finds the amount of delta-f required to move a point onto a
* target line in delta-f/delta-t phase space. Any line is OK
* as long as it's not convex and never returns greater than
* MIN_INIT. */
static double find_shift(double slope, double offset)
{
double shift = slope - offset/600.0;
double shift2 = slope + 0.3 - offset/6000.0;
if (shift2 < shift) shift = shift2;
if (debug) printf("find_shift %f %f -> %f\n", slope, offset, shift);
if (shift < 0) return 0.0;
return shift;
}
static void polygon_point(struct _seg *s)
{
double l, r;
if (debug) printf("loop %f %f\n", s->slope, s->offset);
l = find_shift(- s->slope, s->offset);
r = find_shift( s->slope, - s->offset);
if (l < df.l_min) df.l_min = l;
if (r < df.r_min) df.r_min = r;
if (debug) printf("constraint left: %f %f \n", l, df.l_min);
if (debug) printf("constraint right: %f %f \n", r, df.r_min);
}
/* Something like linear feedback to be used when we are "close" to
* phase lock. Not really used at the moment: the logic in find_df()
* never sets the flag. */
static double find_df_center(struct _seg *min, struct _seg *max, double gross_df)
{
const double crit_time=1000.0;
double slope = 0.5 * (max->slope + min->slope)+gross_df;
double dslope = (max->slope - min->slope);
double offset = 0.5 * (max->offset + min->offset);
double doffset = (max->offset - min->offset);
double delta1 = -offset/ 600.0 - slope;
double delta2 = -offset/1800.0 - slope;
double delta = 0.0;
double factor = crit_time/(crit_time+doffset+dslope*1200.0);
if (offset < 0 && delta2 > 0) delta = delta2;
if (offset < 0 && delta1 < 0) delta = delta1;
if (offset >= 0 && delta1 > 0) delta = delta1;
if (offset >= 0 && delta2 < 0) delta = delta2;
if (max->offset < -crit_time || min->offset > crit_time) return 0.0;
if (debug) printf("find_df_center %f %f\n", delta, factor);
return factor*delta;
}
int contemplate_data(unsigned int absolute, double skew, double errorbar, int freq)
{
/* Here is the actual phase lock loop.
* Need to keep a ring buffer of points to make a rational
* decision how to proceed. if (debug) print a lot.
*/
static int rp=0, valid=0;
int both_sides_now=0;
int j, n, c, max_avail, min_avail, dinit;
int nextj=0; /* initialization not needed; but gcc can't figure out my logic */
double cum;
struct _seg check, save_min, save_max;
double last_slope;
int delta_freq;
double delta_f;
int inconsistent=0, max_imax, max_imin=0, min_imax, min_imin=0;
int computed_freq=freq;
if (debug) printf("xontemplate %u %.1f %.1f %d\n",absolute,skew,errorbar,freq);
d_ring[rp].absolute = absolute;
d_ring[rp].skew = skew;
d_ring[rp].errorbar = errorbar - min_delay; /* quick hack to speed things up */
d_ring[rp].freq = freq;
if (valid<RING_SIZE) ++valid;
if (valid==RING_SIZE) {
/*
* Pass 1: correct for wandering freq's */
cum = 0.0;
if (debug) printf("\n");
for (j=rp; ; j=n) {
d_ring[j].s.s.max = d_ring[j].skew - cum + d_ring[j].errorbar;
d_ring[j].s.s.min = d_ring[j].skew - cum - d_ring[j].errorbar;
if (debug) printf("hist %d %d %f %f %f\n",j,d_ring[j].absolute-absolute,
cum,d_ring[j].s.s.min,d_ring[j].s.s.max);
n=next_dn(j);
if (n == rp) break;
/* Assume the freq change took place immediately after
* the data was taken; this is valid for the case where
* this program was responsible for the change.
*/
cum = cum + (d_ring[j].absolute-d_ring[n].absolute) *
(double)(d_ring[j].freq-freq)/65536;
}
/*
* Pass 2: find the convex down envelope of s.max, composed of
* line segments in s.max vs. absolute space, which are
* points in freq vs. dt space. Find points in order of increasing
* slope == freq */
dinit=1; last_slope=-2*MAX_CORRECT;
for (c=1, j=next_up(rp); ; j=nextj) {
nextj = search(rp, j, 1, 1, 0, &maxseg[c]);
search(rp, j, 0, 1, 1, &check);
if (check.slope < maxseg[c].slope && check.slope > last_slope &&
(dinit || check.slope < save_min.slope)) {dinit=0; save_min=check; }
if (debug) printf("maxseg[%d] = %f *x+ %f\n",
c, maxseg[c].slope, maxseg[c].offset);
last_slope = maxseg[c].slope;
c++;
if (nextj == rp) break;
}
if (dinit==1) inconsistent=1;
if (debug && dinit==0) printf ("mincross %f *x+ %f\n", save_min.slope, save_min.offset);
max_avail=c;
/*
* Pass 3: find the convex up envelope of s.min, composed of
* line segments in s.min vs. absolute space, which are
* points in freq vs. dt space. These points are found in
* order of decreasing slope. */
dinit=1; last_slope=+2*MAX_CORRECT;
for (c=1, j=next_up(rp); ; j=nextj) {
nextj = search(rp, j, 0, 0, 1, &minseg[c]);
search(rp, j, 1, 0, 0, &check);
if (check.slope > minseg[c].slope && check.slope < last_slope &&
(dinit || check.slope < save_max.slope)) {dinit=0; save_max=check; }
if (debug) printf("minseg[%d] = %f *x+ %f\n",
c, minseg[c].slope, minseg[c].offset);
last_slope = minseg[c].slope;
c++;
if (nextj == rp) break;
}
if (dinit==1) inconsistent=1;
if (debug && dinit==0) printf ("maxcross %f *x+ %f\n", save_max.slope, save_max.offset);
min_avail=c;
/*
* Pass 4: splice together the convex polygon that forms
* the envelope of slope/offset coordinates that are consistent
* with the observed data. The order of calls to polygon_point
* doesn't matter for the frequency shift determination, but
* the order chosen is nice for visual display. */
if (!inconsistent) {
polygon_reset();
polygon_point(&save_min);
for (dinit=1, c=1; c<max_avail; c++) {
if (dinit && maxseg[c].slope > save_min.slope) {
max_imin = c-1;
maxseg[max_imin] = save_min;
dinit = 0;
}
if (maxseg[c].slope > save_max.slope)
break;
if (dinit==0) polygon_point(&maxseg[c]);
}
if (dinit && debug) printf("found maxseg vs. save_min inconsistency\n");
if (dinit) inconsistent=1;
max_imax = c;
maxseg[max_imax] = save_max;
polygon_point(&save_max);
for (dinit=1, c=1; c<min_avail; c++) {
if (dinit && minseg[c].slope < save_max.slope) {
max_imin = c-1;
minseg[min_imin] = save_max;
dinit = 0;
}
if (minseg[c].slope < save_min.slope)
break;
if (dinit==0) polygon_point(&minseg[c]);
}
if (dinit && debug) printf("found minseg vs. save_max inconsistency\n");
if (dinit) inconsistent=1;
min_imax = c;
minseg[min_imax] = save_max;
/* not needed for analysis, but shouldn't hurt either */
if (debug) polygon_point(&save_min);
} /* !inconsistent */
/*
* Pass 5: decide on a new freq */
if (inconsistent) {
printf("# inconsistent\n");
} else {
delta_f = find_df(&both_sides_now);
if (debug) printf("find_df() = %e\n", delta_f);
delta_f += find_df_center(&save_min,&save_max, delta_f);
delta_freq = delta_f*65536+.5;
if (debug) printf("delta_f %f delta_freq %d bsn %d\n", delta_f, delta_freq, both_sides_now);
computed_freq -= delta_freq;
printf ("# box [( %.3f , %.1f ) ", save_min.slope, save_min.offset);
printf ( " ( %.3f , %.1f )] ", save_max.slope, save_max.offset);
printf (" delta_f %.3f computed_freq %d\n", delta_f, computed_freq);
if (computed_freq < -MAX_C) computed_freq=-MAX_C;
if (computed_freq > MAX_C) computed_freq= MAX_C;
}
}
rp = (rp+1)%RING_SIZE;
return computed_freq;
}