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