1/* 2 * phaselock.c - Phase locking for NTP client 3 * 4 * Copyright 2000 Larry Doolittle <LRDoolittle@lbl.gov> 5 * Last hack: 28 November, 2000 6 * 7 * This program is free software; you can redistribute it and/or modify 8 * it under the terms of the GNU General Public License (Version 2, 9 * June 1991) as published by the Free Software Foundation. At the 10 * time of writing, that license was published by the FSF with the URL 11 * http://www.gnu.org/copyleft/gpl.html, and is incorporated herein by 12 * reference. 13 * 14 * This program is distributed in the hope that it will be useful, 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 * GNU General Public License for more details. 18 * 19 * Possible future improvements: 20 * - Write and test live mode 21 * - Subtract configurable amount from errorbar 22 * - Sculpt code so it's legible, this version is out of control 23 * - Write documentation :-( 24 */ 25 26#include <stdio.h> 27 28#define ENABLE_DEBUG 29 30#define RING_SIZE 16 31struct datum { 32 unsigned int absolute; 33 double skew; 34 double errorbar; 35 int freq; 36 /* s.s.min and s.s.max (skews) are "corrected" to what they would 37 * have been if freq had been constant at its current value during 38 * the measurements. 39 */ 40 union { 41 struct { double min; double max; } s; 42 double ss[2]; 43 } s; 44 /* 45 double smin; 46 double smax; 47 */ 48} d_ring[RING_SIZE]; 49 50struct _seg { 51 double slope; 52 double offset; 53} maxseg[RING_SIZE+1], minseg[RING_SIZE+1]; 54 55#ifdef ENABLE_DEBUG 56extern int debug; 57#define DEBUG_OPTION "d" 58#else 59#define debug 0 60#define DEBUG_OPTION 61#endif 62 63/* draw a line from a to c, what the offset is of that line 64 * where that line matches b's slope coordinate. 65 */ 66double interpolate(struct _seg *a, struct _seg *b, struct _seg *c) 67{ 68 double x, y; 69 x = (b->slope - a->slope) / (c->slope - a->slope) ; 70 y = a->offset + x * (c->offset - a->offset); 71 return y; 72} 73 74int next_up(int i) { int r = i+1; if (r>=RING_SIZE) r=0; return r;} 75int next_dn(int i) { int r = i-1; if (r<0) r=RING_SIZE-1; return r;} 76 77/* Looks at the line segments that start at point j, that end at 78 * all following points (ending at index rp). The initial point 79 * is on curve s0, the ending point is on curve s1. The curve choice 80 * (s.min vs. s.max) is based on the index in ss[]. The scan 81 * looks for the largest (sign=0) or smallest (sign=1) slope. 82 */ 83int search(int rp, int j, int s0, int s1, int sign, struct _seg *answer) 84{ 85 double dt, slope; 86 int n, nextj=0, cinit=1; 87 for (n=next_up(j); n!=next_up(rp); n=next_up(n)) { 88 if (0 && debug) printf("d_ring[%d].s.ss[%d]=%f d_ring[%d].s.ss[%d]=%f\n", 89 n, s0, d_ring[n].s.ss[s0], j, s1, d_ring[j].s.ss[s1]); 90 dt = d_ring[n].absolute - d_ring[j].absolute; 91 slope = (d_ring[n].s.ss[s0] - d_ring[j].s.ss[s1]) / dt; 92 if (0 && debug) printf("slope %d%d%d [%d,%d] = %f\n", 93 s0, s1, sign, j, n, slope); 94 if (cinit || (slope < answer->slope) ^ sign) { 95 answer->slope = slope; 96 answer->offset = d_ring[n].s.ss[s0] + 97 slope*(d_ring[rp].absolute - d_ring[n].absolute); 98 cinit = 0; 99 nextj = n; 100 } 101 } 102 return nextj; 103} 104 105/* Pseudo-class for finding consistent frequency shift */ 106#define MIN_INIT 20 107struct _polygon { 108 double l_min; 109 double r_min; 110} df; 111 112void polygon_reset() 113{ 114 df.l_min = MIN_INIT; 115 df.r_min = MIN_INIT; 116} 117 118double find_df(int *flag) 119{ 120 if (df.l_min == 0.0) { 121 if (df.r_min == 0.0) { 122 return 0.0; /* every point was OK */ 123 } else { 124 return -df.r_min; 125 } 126 } else { 127 if (df.r_min == 0.0) { 128 return df.l_min; 129 } else { 130 if (flag) *flag=1; 131 return 0.0; /* some points on each side, 132 * or no data at all */ 133 } 134 } 135} 136 137/* Finds the amount of delta-f required to move a point onto a 138 * target line in delta-f/delta-t phase space. Any line is OK 139 * as long as it's not convex and never returns greater than 140 * MIN_INIT. */ 141double find_shift(double slope, double offset) 142{ 143 double shift = slope - offset/600.0; 144 double shift2 = slope + 0.3 - offset/6000.0; 145 if (shift2 < shift) shift = shift2; 146 if (debug) printf("find_shift %f %f -> %f\n", slope, offset, shift); 147 if (shift < 0) return 0.0; 148 return shift; 149} 150 151void polygon_point(struct _seg *s) 152{ 153 double l, r; 154 if (debug) printf("loop %f %f\n", s->slope, s->offset); 155 l = find_shift(- s->slope, s->offset); 156 r = find_shift( s->slope, - s->offset); 157 if (l < df.l_min) df.l_min = l; 158 if (r < df.r_min) df.r_min = r; 159 if (debug) printf("constraint left: %f %f \n", l, df.l_min); 160 if (debug) printf("constraint right: %f %f \n", r, df.r_min); 161} 162 163/* Something like linear feedback to be used when we are "close" to 164 * phase lock. Not really used at the moment: the logic in find_df() 165 * never sets the flag. */ 166double find_df_center(struct _seg *min, struct _seg *max) 167{ 168 double slope = 0.5 * (max->slope + min->slope); 169 double dslope = (max->slope - min->slope); 170 double offset = 0.5 * (max->offset + min->offset); 171 double doffset = (max->offset - min->offset); 172 double delta = -offset/600.0 - slope; 173 double factor = 60.0/(doffset+dslope*600.0); 174 if (debug) printf("find_df_center %f %f\n", delta, factor); 175 return factor*delta; 176} 177 178int contemplate_data(unsigned int absolute, double skew, double errorbar, int freq) 179{ 180 /* Here is the actual phase lock loop. 181 * Need to keep a ring buffer of points to make a rational 182 * decision how to proceed. if (debug) print a lot. 183 */ 184 static int rp=0, valid=0; 185 int both_sides_now=0; 186 int j, n, c, max_avail, min_avail, dinit; 187 int nextj=0; /* initialization not needed; but gcc can't figure out my logic */ 188 double cum; 189 struct _seg check, save_min, save_max; 190 double last_slope; 191 int delta_freq; 192 double delta_f; 193 int inconsistent=0, max_imax, max_imin=0, min_imax, min_imin=0; 194 int computed_freq=freq; 195 196 if (debug) printf("xontemplate %u %.1f %.1f %d\n",absolute,skew,errorbar,freq); 197 d_ring[rp].absolute = absolute; 198 d_ring[rp].skew = skew; 199 d_ring[rp].errorbar = errorbar - 800.0; /* quick hack to speed things up */ 200 d_ring[rp].freq = freq; 201 202 if (valid<RING_SIZE) ++valid; 203 if (valid==RING_SIZE) { 204 /* 205 * Pass 1: correct for wandering freq's */ 206 cum = 0.0; 207 if (debug) printf("\n"); 208 for (j=rp; ; j=n) { 209 d_ring[j].s.s.max = d_ring[j].skew - cum + d_ring[j].errorbar; 210 d_ring[j].s.s.min = d_ring[j].skew - cum - d_ring[j].errorbar; 211 if (debug) printf("hist %d %d %f %f %f\n",j,d_ring[j].absolute-absolute, 212 cum,d_ring[j].s.s.min,d_ring[j].s.s.max); 213 n=next_dn(j); 214 if (n == rp) break; 215 /* Assume the freq change took place immediately after 216 * the data was taken; this is valid for the case where 217 * this program was responsible for the change. 218 */ 219 cum = cum + (d_ring[j].absolute-d_ring[n].absolute) * 220 (double)(d_ring[j].freq-freq)/65536; 221 } 222 /* 223 * Pass 2: find the convex down envelope of s.max, composed of 224 * line segments in s.max vs. absolute space, which are 225 * points in freq vs. dt space. Find points in order of increasing 226 * slope == freq */ 227 dinit=1; last_slope=-100; 228 for (c=1, j=next_up(rp); ; j=nextj) { 229 nextj = search(rp, j, 1, 1, 0, &maxseg[c]); 230 search(rp, j, 0, 1, 1, &check); 231 if (check.slope < maxseg[c].slope && check.slope > last_slope && 232 (dinit || check.slope < save_min.slope)) {dinit=0; save_min=check; } 233 if (debug) printf("maxseg[%d] = %f *x+ %f\n", 234 c, maxseg[c].slope, maxseg[c].offset); 235 last_slope = maxseg[c].slope; 236 c++; 237 if (nextj == rp) break; 238 } 239 if (dinit==1) inconsistent=1; 240 if (debug && dinit==0) printf ("mincross %f *x+ %f\n", save_min.slope, save_min.offset); 241 max_avail=c; 242 /* 243 * Pass 3: find the convex up envelope of s.min, composed of 244 * line segments in s.min vs. absolute space, which are 245 * points in freq vs. dt space. These points are found in 246 * order of decreasing slope. */ 247 dinit=1; last_slope=+100.0; 248 for (c=1, j=next_up(rp); ; j=nextj) { 249 nextj = search(rp, j, 0, 0, 1, &minseg[c]); 250 search(rp, j, 1, 0, 0, &check); 251 if (check.slope > minseg[c].slope && check.slope < last_slope && 252 (dinit || check.slope < save_max.slope)) {dinit=0; save_max=check; } 253 if (debug) printf("minseg[%d] = %f *x+ %f\n", 254 c, minseg[c].slope, minseg[c].offset); 255 last_slope = minseg[c].slope; 256 c++; 257 if (nextj == rp) break; 258 } 259 if (dinit==1) inconsistent=1; 260 if (debug && dinit==0) printf ("maxcross %f *x+ %f\n", save_max.slope, save_max.offset); 261 min_avail=c; 262 /* 263 * Pass 4: splice together the convex polygon that forms 264 * the envelope of slope/offset coordinates that are consistent 265 * with the observed data. The order of calls to polygon_point 266 * doesn't matter for the frequency shift determination, but 267 * the order chosen is nice for visual display. */ 268 polygon_reset(); 269 polygon_point(&save_min); 270 for (dinit=1, c=1; c<max_avail; c++) { 271 if (dinit && maxseg[c].slope > save_min.slope) { 272 max_imin = c-1; 273 maxseg[max_imin] = save_min; 274 dinit = 0; 275 } 276 if (maxseg[c].slope > save_max.slope) 277 break; 278 if (dinit==0) polygon_point(&maxseg[c]); 279 } 280 if (dinit && debug) printf("found maxseg vs. save_min inconsistency\n"); 281 if (dinit) inconsistent=1; 282 max_imax = c; 283 maxseg[max_imax] = save_max; 284 285 polygon_point(&save_max); 286 for (dinit=1, c=1; c<min_avail; c++) { 287 if (dinit && minseg[c].slope < save_max.slope) { 288 max_imin = c-1; 289 minseg[min_imin] = save_max; 290 dinit = 0; 291 } 292 if (minseg[c].slope < save_min.slope) 293 break; 294 if (dinit==0) polygon_point(&minseg[c]); 295 } 296 if (dinit && debug) printf("found minseg vs. save_max inconsistency\n"); 297 if (dinit) inconsistent=1; 298 min_imax = c; 299 minseg[min_imax] = save_max; 300 301 /* not needed for analysis, but shouldn't hurt either */ 302 if (debug) polygon_point(&save_min); 303 304 /* 305 * Pass 5: decide on a new freq */ 306 if (inconsistent) { 307 printf("# inconsistent\n"); 308 } else { 309 delta_f = find_df(&both_sides_now); 310 if (debug) printf("find_df() = %e\n", delta_f); 311 if (both_sides_now) 312 delta_f = find_df_center(&save_min,&save_max); 313 delta_freq = delta_f*65536+.5; 314 if (debug) printf("delta_f %f delta_freq %d bsn %d\n", delta_f, delta_freq, both_sides_now); 315 computed_freq -= delta_freq; 316 printf ("# box [( %.3f , %.1f ) ", save_min.slope, save_min.offset); 317 printf ( " ( %.3f , %.1f )] ", save_max.slope, save_max.offset); 318 printf (" delta_f %.3f computed_freq %d\n", delta_f, computed_freq); 319 320 if (computed_freq < -6000000) computed_freq=-6000000; 321 if (computed_freq > 6000000) computed_freq= 6000000; 322 } 323 } 324 rp = (rp+1)%RING_SIZE; 325 return computed_freq; 326} 327