Add frequency resampling

drift.py

@@ -50,19 +50,19 @@ class Interpolator(object):
         h = np.sinc(u / resolution) * window
         self.filt = []
         for index in range(resolution):  # split into multiphase filters
-            self.filt.append(h[index::resolution])
+            filt = h[index::resolution]
+            filt = filt[::-1]
+            self.filt.append(filt)
         lengths = map(len, self.filt)
         assert set(lengths) == set([2*width])
         assert len(self.filt) == resolution
 
     def get(self, offset):
+        # offset = k + (j / self.resolution)
         k = int(offset)
-        j = np.round((offset - k) * self.resolution)
-        index = (self.resolution - int(j)) % self.resolution
-        coeffs = self.filt[index]
-
-        offset = int(np.ceil(offset))
-        return coeffs, offset - self.width
+        j = int((offset - k) * self.resolution)
+        coeffs = self.filt[j]
+        return coeffs, k - self.width
 
 class Sampler(object):
     def __init__(self, src, interp=None):
@@ -76,12 +76,19 @@ class Sampler(object):
     def sample(self):
         coeffs, begin = self.interp.get(self.offset)
         end = begin + len(coeffs)
-        for s in self.src:
-            self.buff.append(s)
-            self.index += 1
+        # C = ', '.join(['%.3f' % c for c in coeffs[18:23]])
+        # print '%.3f [%s] %d %d' % (self.offset, C, begin, end)
+        while True:
             if self.index == end:
                 self.buff = self.buff[-len(coeffs):]
                 return np.dot(coeffs, self.buff)
+            try:
+                s = self.src.next()
+            except StopIteration:
+                break
+
+            self.buff.append(s)
+            self.index += 1
 
     def next(self):
         self.offset += self.freq
@@ -95,46 +102,40 @@ def main():
         x = x[100:]
         y = []
         sampler = Sampler(x)
-        sampler.freq = 1
+        sampler.freq = 1.0 + 0.112/f0
         while True:
             u = sampler.sample()
             if u is None:
                 break
             y.append(u)
             sampler.next()
-        x = np.array(y)
-
-        S = recv.extract_symbols(x, f0)
+        x_ = np.array(y)
+        S = recv.extract_symbols(x_, f0)
         S = np.array(list(S))
 
         y = S #np.array(list(calib(S)))
-        pylab.subplot(1,2,1)
-        pylab.plot(y.real, y.imag, '.'); pylab.axis('equal')
-        pylab.subplot(1,2,2)
         phase = np.unwrap(np.angle(y))
 
-        phase_error_per_1ms = (phase[-1] - phase[0]) / (len(phase) - 1)
+        phase_error = (phase[-1] - phase[0])
+        phase_error_per_1ms =  phase_error / (len(phase) - 1)
         freq_error = phase_error_per_1ms * 1000.0 / (2 * np.pi)
+        print phase_error, len(phase)
+        print phase_error_per_1ms
         print freq_error
-        pylab.plot(phase)
-        pylab.grid('on')
-        #pylab.show()
+
+        if 1:
+            pylab.figure()
+            pylab.plot(y.real, y.imag, '.')
+            pylab.grid('on')
+        pylab.show()
         return
 
-    t = np.arange(64) * common.Ts
-    x = np.sin(2 * np.pi * 3.456e3 * t)
-    r = Interpolator()
-
-    # pylab.figure()
-    y = []
-    k = 32 + np.linspace(-5, 5, 1001)
-    for offset in k:
-        h, i = r.get(offset)
-        y.append( np.dot(x[i:i+len(h)], h) )
-
+    I = Interpolator()
+    f = I.filt
     pylab.figure()
-    pylab.plot(t, x, '.', k * common.Ts, y, '-')
+    pylab.plot(zip(*f[::100]))
     pylab.show()
 
 if __name__ == '__main__':
     main()
+