diff options
Diffstat (limited to 'app/pll.c')
| -rw-r--r-- | app/pll.c | 126 |
1 files changed, 108 insertions, 18 deletions
@@ -2,7 +2,7 @@ #define JUMP_THRESH 0.1 #define JUMP_TICKS 30 -#define FEEDBACK 0.01 +#define FEEDBACK 0.0001 #define WARM_UP 30 @@ -15,8 +15,56 @@ 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) + + + + -static int warming_up = WARM_UP; void pll_meh (void) @@ -32,10 +80,15 @@ 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 @@ -47,10 +100,27 @@ static void modify_pll_freq (uint64_t now, int d) phase = now - pd2; } +uint64_t make_happy (uint64_t abs, int64_t shift) +{ + shift *= HZ; + + if (shift < 0) { + shift = -shift; + + if (abs < (uint64_t) shift) return 0; + else + return abs - shift; + } + + return abs + shift; + + +} + void pll_dispatch (uint64_t happy, uint64_t edge, const char *src) { - double f; + double f, g; int64_t pd; @@ -59,23 +129,40 @@ void pll_dispatch (uint64_t happy, uint64_t edge, const char *src) { 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; + 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 ((!gps_last_happy) && (!dcf77_last_happy) && (!msf_last_happy)) return; if (happy < gps_last_happy) return; @@ -83,14 +170,17 @@ if ((!gps_last_happy) && (!dcf77_last_happy) && (!msf_last_happy)) return; if (happy < msf_last_happy) return; - if (warming_up) { - warming_up--; + if (!pll_ready && (edge < ((uint64_t) WARM_UP * (uint64_t) HZ))) return; - } -printf("EDGE %08x%08x\r\n", - (uint32_t) (edge >>32), - (uint32_t) (edge & 0xffffffff)); + led_blink (100); + + +#if 1 + printf ("EDGE %08x%08x\r\n", + (unsigned) (edge >> 32), + (unsigned) (edge & 0xffffffff)); +#endif { int diff, hf; @@ -107,27 +197,27 @@ printf("EDGE %08x%08x\r\n", f = (double) diff; - f /= (double) pll_freq; + g = f / (double) pll_freq; } - if ((f > (JUMP_THRESH)) || (f < - (JUMP_THRESH))) + 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 pll_freq %d phase %d %s\r\n", (float) f, (int) pll_freq, (int) phase, src); + 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) { + if ((out_of_lock > JUMP_TICKS) || !pll_ready) { phase += pd; out_of_lock = 0; printf ("PLL - jumping\r\n"); pll_freq = HZ; + pll_ready = 1; } else { - f *= FEEDBACK; - f *= (double) pll_freq; + phase += (int) (f * PLL_BETA); + modify_pll_freq (edge, (int) (f * PLL_ALPHA)); - modify_pll_freq (edge, (int) f); } pll_last_update = edge; |