blob: 4b891437133f0b881648b0fa5921e93829c818be (
plain)
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
|
#include "project.h"
#define JUMP_THRESH 0.1
#define JUMP_TICKS 30
#define FEEDBACK 0.001
static int64_t offset;
static int64_t phase;
static int64_t freq = HZ;
static int out_of_lock = JUMP_TICKS + 1;
uint64_t pll_last_update;
int pll_valid = 0;
static void modify_freq (uint64_t now, int d)
{
int64_t pd1, pd2, te;
pd1 = now - phase;
te = pd1 / freq;
pd1 %= freq;
if (d > 0)
freq += d;
else
freq -= -d;
pd2 = pd1 + (te * freq);
phase = now - pd2;
}
void pll_dispatch (uint64_t edge)
{
double f;
int64_t pd;
{
int diff, hf;
pd = edge - phase;
pd %= freq;
hf = (int) (freq >> 1);
diff = (int) pd;
if (diff > hf)
diff = diff - (int) freq;
f = (double) diff;
f /= (double) freq;
}
if ((f > (JUMP_THRESH)) || (f < - (JUMP_THRESH)))
out_of_lock++;
else
out_of_lock = 0;
#if 0
printf ("PLL pd %.3f freq %d phase %d\r\n", (float) f, (int) freq, (int) phase);
#endif
if (out_of_lock > JUMP_TICKS) {
phase += pd;
out_of_lock = 0;
printf ("PLL - jumping\r\n");
freq = HZ;
} else {
f *= FEEDBACK;
f *= (double) freq;
modify_freq (edge, (int) f);
}
pll_last_update = edge;
}
void pll_set_offset (EPOCH epoch, uint64_t abs)
{
/* Find nearest second to abs*/
abs += freq >> 2;
abs -= phase;
abs /= freq;
offset = epoch.s - abs;
pll_valid = 1;
}
EPOCH pll_decompose (uint64_t abs)
{
EPOCH ret;
abs -= phase;
ret.s = abs / freq;
abs -= freq * ret.s;
ret.s += offset;
abs *= (uint64_t) 1000000000;
abs = abs / freq;
ret.ns = abs;
return ret;
}
|
