1 files changed, 55 insertions, 27 deletions

diff --git a/app/pll.c b/app/pll.c
index 4b89143..03d214c 100644
--- a/app/pll.c
+++ b/app/pll.c
@@ -2,12 +2,13 @@
 
 #define JUMP_THRESH 0.1
 #define JUMP_TICKS  30
-#define FEEDBACK    0.001
+#define FEEDBACK    0.000001
+#define WARM_UP     30
 
 
 static int64_t offset;
 static int64_t phase;
-static int64_t freq = HZ;
+int64_t pll_freq = HZ;
 
 static int out_of_lock = JUMP_TICKS + 1;
 
@@ -15,23 +16,26 @@ uint64_t pll_last_update;
 int pll_valid = 0;
 
 
+static int warming_up = WARM_UP;
 
-static void modify_freq (uint64_t now, int d)
+
+
+static void modify_pll_freq (uint64_t now, int d)
 {
   int64_t pd1, pd2, te;
 
   pd1 = now - phase;
 
-  te = pd1 / freq;
-  pd1 %= freq;
+  te = pd1 / pll_freq;
+  pd1 %= pll_freq;
 
   if (d > 0)
-    freq += d;
+    pll_freq += d;
   else
-    freq -= -d;
+    pll_freq -= -d;
 
 
-  pd2 = pd1 + (te * freq);
+  pd2 = pd1 + (te * pll_freq);
 
   phase = now - pd2;
 }
@@ -41,44 +45,49 @@ void pll_dispatch (uint64_t edge)
   double f;
   int64_t pd;
 
+  if (warming_up) {
+    warming_up--;
+    return;
+  }
+
   {
     int diff, hf;
 
     pd = edge - phase;
-    pd %= freq;
+    pd %= pll_freq;
 
-    hf = (int) (freq >> 1);
+    hf = (int) (pll_freq >> 1);
 
     diff = (int) pd;
 
     if (diff > hf)
-      diff = diff - (int) freq;
+      diff = diff - (int) pll_freq;
 
 
     f = (double) diff;
-    f /= (double) freq;
+    f /= (double) pll_freq;
   }
 
   if ((f > (JUMP_THRESH)) || (f < - (JUMP_THRESH)))
     out_of_lock++;
-  else
+  else if (out_of_lock <= JUMP_TICKS)
     out_of_lock = 0;
 
 #if 0
-  printf ("PLL pd %.3f freq %d phase %d\r\n", (float) f, (int) freq, (int) phase);
+  printf ("PLL pd %.3f pll_freq %d phase %d\r\n", (float) f, (int) pll_freq, (int) phase);
 #endif
 
   if (out_of_lock > JUMP_TICKS) {
     phase += pd;
     out_of_lock = 0;
     printf ("PLL - jumping\r\n");
-    freq = HZ;
+    pll_freq = HZ;
   } else {
 
     f *= FEEDBACK;
-    f *= (double) freq;
+    f *= (double) pll_freq;
 
-    modify_freq (edge, (int) f);
+    modify_pll_freq (edge, (int) f);
 
   }
 
@@ -90,9 +99,9 @@ void pll_dispatch (uint64_t edge)
 void pll_set_offset (EPOCH epoch, uint64_t abs)
 {
   /* Find nearest second to abs*/
-  abs += freq >> 2;
+  abs += pll_freq >> 2;
   abs -= phase;
-  abs /= freq;
+  abs /= pll_freq;
 
   offset = epoch.s - abs;
 
@@ -101,23 +110,42 @@ void pll_set_offset (EPOCH epoch, uint64_t abs)
 
 
 
-
-EPOCH pll_decompose (uint64_t abs)
+EPOCH _pll_decompose (uint64_t abs)
 {
   EPOCH ret;
 
-  abs -= phase;
-
-  ret.s = abs / freq;
-  abs -= freq * ret.s;
-
+  ret.s = abs / pll_freq;
+  abs -= pll_freq * ret.s;
 
   ret.s += offset;
 
   abs *= (uint64_t) 1000000000;
-  abs = abs / freq;
+  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);
+}