diff options
Diffstat (limited to 'app/pll.c')
| -rw-r--r-- | app/pll.c | 293 |
1 files changed, 40 insertions, 253 deletions
@@ -1,293 +1,80 @@ #include "project.h" - -#define JUMP_THRESH 0.1 -#define JUMP_TICKS 30 -#define FEEDBACK 0.0001 #define WARM_UP 30 - -static int64_t offset; -static int64_t phase; -int64_t pll_freq = HW_CLOCK_HZ; - -static int out_of_lock = JUMP_TICKS + 1; - -uint64_t pll_last_update; -int pll_valid = 0; - -int pll_ready = 0; - - -# if 0 -#define PLL_BW 0.01 -#define PLL_DAMP 0.707 -#define PLL_GAIN 0.001 - -#define F_T1 ((PLL_GAIN)/((PLL_BW)*(PLL_BW))) -#define F_T2 ((2*(PLL_DAMP))/(PLL_BW)) - - -#define F_B0 (((4*(PLL_GAIN))/(F_T1))*(1.0+((F_T2)/(2.0)))) -#define F_B1 ((8*(PLL_GAIN))/(F_T1)) -#define F_B2 (((4*(PLL_GAIN))/(F_T1))*(1.0-((F_T2)/(2.0)))) - -#define F_A0 (1.0) -#define F_A1 (-2.0) -#define F_A2 (1.0) - -void pll_dump_filter (void) -{ - printf ("%g %g %g\n", F_A0, F_A1, F_A2); - printf ("%g %g %g\n", F_B0, F_B1, F_B2); -} - - - -static double filter (double in) -{ - static double v[3]; - double ret; - - v[2] = v[1]; - v[1] = v[0]; - v[0] = in - (v[1] * F_A1) - (v[2] * F_A2); - - return (v[0] * F_B0) + (v[1] * F_B1) + (v[2] * F_B2); - -} -#endif - - -#define PLL_ALPHA (0.005) -#define PLL_BETA (0.5*PLL_ALPHA*PLL_ALPHA) - - - - - - - -void pll_meh (void) -{ - printf (" %"PRId64" %"PRId64" %" PRId64 "\r\n", - pll_freq, offset, phase); - -} - - -static void modify_pll_freq (uint64_t now, int d) -{ - int64_t pd1, pd2, te; - - pd1 = now - phase; - te = pd1 / pll_freq; - pd1 %= pll_freq; - - if (pd1 > (pll_freq >> 1)) { - te++; - pd1 = pd1 - pll_freq; - } - - - if (d > 0) - pll_freq += d; - else - pll_freq -= -d; - - - pd2 = pd1 + (te * pll_freq); - - phase = now - pd2; -} - -uint64_t make_happy (uint64_t abs, int64_t shift) +void pll_set_offset (EPOCH epoch, uint64_t abs, uint64_t ptp) { - shift *= HW_CLOCK_HZ; - - if (shift < 0) { - shift = -shift; - - if (abs < (uint64_t) shift) return 0; - else - return abs - shift; - } - - return abs + shift; - + ref_set_offset (epoch, abs); + ptp_set_offset (epoch, ptp); } -void pll_dispatch (uint64_t happy, uint64_t edge, const char *src) +void pll_dispatch (uint64_t happy, uint64_t edge, uint64_t ptp_edge, const char *src) { - double f, g; - int64_t pd; - - - -#if 0 - { - int h1, h2, h3, h4; - EPOCH e; - UTC u; - char s1[80]; - char s2[80]; - char s3[80]; - char s4[80]; - - - e = pll_decompose (happy); - u = time_epoch_to_utc (e); - utc_to_str (s1, u); - h1 = e.s; + // int64_t delta_f; - e = pll_decompose (gps_last_happy); - u = time_epoch_to_utc (e); - utc_to_str (s2, u); - h2 = e.s; - - e = pll_decompose (dcf77_last_happy); - u = time_epoch_to_utc (e); - utc_to_str (s3, u); - h3 = e.s; - - e = pll_decompose (msf_last_happy); - u = time_epoch_to_utc (e); - utc_to_str (s4, u); - h4 = e.s; - - printf ("H %d %d %d %d\r\n", h1 - h2, h2 - h2, h3 - h2, h4 - h2); - // printf ("H %s %s %s %s\r\n",s1,s2,s3,s4); - } -#endif - - - if ((!gps_last_happy) && (!dcf77_last_happy) && (!msf_last_happy)) return; - - if (happy < gps_last_happy) return; - - if (happy < dcf77_last_happy) return; - - if (happy < msf_last_happy) return; - - if (!pll_ready && (edge < ((uint64_t) WARM_UP * (uint64_t) HW_CLOCK_HZ))) + if (((!gps_last_happy) && (!dcf77_last_happy) && (!msf_last_happy)) || + (happy < gps_last_happy) || + (happy < dcf77_last_happy) || + (happy < msf_last_happy) || + (edge < ((uint64_t) WARM_UP * (uint64_t) HW_CLOCK_HZ))) return; led_blink (100); -#if 0 - printf ("EDGE %08x%08x\r\n", - (unsigned) (edge >> 32), - (unsigned) (edge & 0xffffffff)); -#endif - - { - int diff, hf; - - pd = edge - phase; - pd %= pll_freq; - - hf = (int) (pll_freq >> 1); - - diff = (int) pd; - - if (diff > hf) - diff = diff - (int) pll_freq; - - - f = (double) diff; - g = f / (double) pll_freq; - } - - if ((g > (JUMP_THRESH)) || (g < - (JUMP_THRESH))) - out_of_lock++; - else if (out_of_lock <= JUMP_TICKS) - out_of_lock = 0; - - printf ("PLL pd %.3f %.1f pll_freq %d phase %d %s\r\n", (float) g, (float)f, (int) pll_freq, (int) phase, src); - - if ((out_of_lock > JUMP_TICKS) || !pll_ready) { - phase += pd; - out_of_lock = 0; - printf ("PLL - jumping\r\n"); - pll_freq = HW_CLOCK_HZ; - pll_ready = 1; - } else { - - phase += (int) (f * PLL_BETA); - modify_pll_freq (edge, (int) (f * PLL_ALPHA)); - - } - - pll_last_update = edge; + ref_dispatch (edge, src); + ptp_dispatch (ptp_edge, src); } - - -void pll_set_offset (EPOCH epoch, uint64_t abs) +void pll_check (void) { - int64_t new_offset; - int diff; + uint64_t ptp = ptp_get(); + uint64_t ref = ref_get(); + EPOCH re, pe; + UTC ru, pu; + static unsigned m; - /* Find nearest second to abs*/ - abs += pll_freq >> 2; - abs -= phase; - abs /= pll_freq; - new_offset = epoch.s - abs; + re = ref_decompose (ref); + ru = time_epoch_to_utc (re); - if (new_offset != offset) { - diff = (int) (new_offset - offset); + if (m == ru.minute) return; - printf ("PLL wallclock offset moved by %d\r\n", diff); - offset = new_offset; - } + m = ru.minute; - pll_valid = 1; - time_known = 1; -} + pe = ptp_decompose (ptp); + pu = time_epoch_to_utc (pe); -EPOCH _pll_decompose (uint64_t abs) -{ - EPOCH ret; - ret.s = abs / pll_freq; - abs -= pll_freq * ret.s; + printf ("PLL checks: R %04d-%02d-%02d %02d:%02d:%02d.%09d P %04d-%02d-%02d %02d:%02d:%02d.%09d\r\n", + ru.year, + ru.month + 1, + ru.mday, + ru.hour, + ru.minute, + ru.second, + ru.nanosecond, + pu.year, + pu.month + 1, + pu.mday, + pu.hour, + pu.minute, + pu.second, + pu.nanosecond); - ret.s += offset; - abs *= (uint64_t) 1000000000; - abs = abs / pll_freq; - ret.ns = abs; - return ret; -} -EPOCH pll_decompose_diff (int64_t diff) -{ - EPOCH ret; - if (diff >= 0) - return _pll_decompose (diff); - ret = _pll_decompose (-diff); - ret.s = -ret.s; - ret.ns = -ret.ns; - - return ret; -} -EPOCH pll_decompose (uint64_t abs) -{ +} - abs -= phase; - return _pll_decompose (abs); -} |