From bc067c8bbd3330f9648128b3223427145ed9e921 Mon Sep 17 00:00:00 2001
From: Roman Zeyde <roman.zeyde@gmail.com>
Date: Thu, 19 Jun 2014 17:59:51 +0300
Subject: [PATCH] Add QPSK16 modem

---
 .gitignore |   2 +
 common.py  |  69 +++++++++++++++++++
 pll.py     |  33 +++++++++
 recv.py    | 196 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 send.py    |  72 ++++++++++++++++++++
 sigproc.py |  53 +++++++++++++++
 6 files changed, 425 insertions(+)
 create mode 100644 .gitignore
 create mode 100644 common.py
 create mode 100644 pll.py
 create mode 100644 recv.py
 create mode 100644 send.py
 create mode 100644 sigproc.py

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..0d0c7da
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+*.int16
+*.pyc
diff --git a/common.py b/common.py
new file mode 100644
index 0000000..d57e16e
--- /dev/null
+++ b/common.py
@@ -0,0 +1,69 @@
+import hashlib
+import struct
+import logging
+log = logging.getLogger(__name__)
+
+Fs = 32e3
+Ts = 1.0 / Fs
+
+Fc = 9e3
+Tc = 1.0 / Fc
+
+Tsym = 1e-3
+Nsym = int(Tsym / Fs)
+
+F0 = Fc - 1e3
+F1 = Fc + 1e3
+Nsym = int(Tsym / Ts)
+baud = int(1/Tsym)
+
+scaling = 10000.0
+
+LENGTH_FORMAT = '<I'
+
+def pack(data):
+    log.info('Sending {} bytes, checksum: {}'.format(len(data), checksum(data)))
+    data = struct.pack(LENGTH_FORMAT, len(data)) + data
+    return data
+
+
+def unpack(data):
+    length_size = struct.calcsize(LENGTH_FORMAT)
+    length, data = data[:length_size], data[length_size:]
+    length, = struct.unpack(LENGTH_FORMAT, length)
+    data = data[:length]
+    log.info('Decoded {} bytes, leftover: {}, checksum: {}'.format(len(data), len(data)-length, checksum(data)))
+    return data
+
+def checksum(data):
+    return '\033[0;32m{}\033[0m'.format(hashlib.sha256(data).hexdigest())
+
+
+def to_bits(chars):
+    for c in chars:
+        val = ord(c)
+        for i in range(8):
+            mask = 1 << i
+            yield (1 if (val & mask) else 0)
+
+
+def to_bytes(bits):
+    assert len(bits) == 8
+    byte = sum(b << i for i, b in enumerate(bits))
+    return chr(byte)
+
+if __name__ == '__main__':
+
+    import pylab
+    def plot(f):
+        t = pylab.linspace(0, Tsym, 1e3)
+        x = pylab.sin(2 * pylab.pi * f * t)
+        pylab.plot(t / Tsym, x)
+        t = pylab.linspace(0, Tsym, Nsym + 1)
+        x = pylab.sin(2 * pylab.pi * f * t)
+        pylab.plot(t / Tsym, x, '.k')
+
+    plot(Fc)
+    plot(F0)
+    plot(F1)
+    pylab.show()
diff --git a/pll.py b/pll.py
new file mode 100644
index 0000000..3c88869
--- /dev/null
+++ b/pll.py
@@ -0,0 +1,33 @@
+import numpy as np
+
+class Loop(object):
+    def __init__(self, Kp, Ki, Fs, output=0):
+        Ts = 1.0 / Fs
+        self.coeffs = [Ki*Ts/2 - Kp, Ki*Ts/2 + Kp]
+        self.output = output
+        self.inputs = [0]
+
+    def handle(self, err):
+        self.inputs.append(err)
+        self.output = self.output + np.dot(self.inputs, self.coeffs)
+        self.inputs.pop(0)
+        return self.output
+
+
+class PLL(object):
+    def __init__(self, freq, Fs, filt, phase=0):
+        self.freq = freq
+        self.phase = phase
+        self.Ts = 1.0/Fs
+        self.filt = filt
+
+    def handle(self, sample):
+        self.pred = np.cos(self.phase)
+        self.quad = np.cos(self.phase)
+        err = self.quad * sample
+        self.freq += self.filt(err)
+        self.phase += self.freq * self.Ts
+
+
+def test():
+    pass
diff --git a/recv.py b/recv.py
new file mode 100644
index 0000000..78273a3
--- /dev/null
+++ b/recv.py
@@ -0,0 +1,196 @@
+import numpy as np
+from numpy import linalg
+import pylab
+
+import sys
+import struct
+import logging
+logging.basicConfig(level=0, format='%(message)s')
+log = logging.getLogger(__name__)
+
+import sigproc
+from common import *
+
+NFFT = 256
+COHERENCE_THRESHOLD = 0.9
+
+CARRIER_DURATION = 300
+CARRIER_THRESHOLD = int(0.9 * CARRIER_DURATION)
+
+def load(fname):
+    x = np.fromfile(open(fname, 'rb'), dtype='int16') / scaling
+    t = np.arange(len(x)) / Fs
+    return t, x
+
+def show(t, x):
+    ax1 = pylab.subplot(211)
+    pylab.plot(t, x)
+
+    pylab.subplot(212, sharex=ax1)
+    Pxx, freqs, bins, im = pylab.specgram(x,
+        NFFT=NFFT, Fs=Fs, noverlap=NFFT/2, cmap=pylab.cm.gist_heat)
+
+    pylab.show()
+
+def norm(x):
+    return np.sqrt(np.dot(x.conj(), x).real)
+
+def test_coherence(t, x, f, start=None, end=None):
+    if start is None:
+        start = -np.inf
+    if end is None:
+        end = np.inf
+
+    I = (start <= t) & (t < end)
+    t = t[I]
+    x = x[I]
+
+    x = x / norm(x)
+    ph = 2*np.pi*f*t
+    cos = np.cos(ph); cos = cos / norm(cos)
+    sin = np.sin(ph); sin = sin / norm(sin)
+    coeff = np.dot(cos, x)**2 + np.dot(sin, x)**2
+    log.info('{:5.1f} kHz: {:.1f}%'.format(f / 1e3, coeff * 100))
+
+def test(t, x):
+    test_coherence(t, x, f=Fc, start=0.1, end=0.11)
+    test_coherence(t, x, f=F0, start=0.32, end=0.4)
+    test_coherence(t, x, f=F1, start=0.42, end=0.5)
+
+def iterate(x, bufsize, offset=0, advance=1, func=None):
+    while True:
+        buf = x[offset:offset+bufsize]
+        if len(buf) == bufsize:
+            result = func(buf) if func else buf
+            yield offset, result
+        else:
+            return
+        offset += advance
+
+def exp_iwt(freq, n):
+    iw = 2j * np.pi * freq
+    t = np.arange(n) * Ts
+    return np.exp(iw * t)
+
+def coherence(x, freq):
+    n = len(x)
+    Hc = exp_iwt(freq, n) / np.sqrt(0.5*n)
+    return np.dot(Hc, x) / norm(x)
+
+def detect(freq):
+    counter = 0
+    for offset, coeff in iterate(x, Nsym, advance=Nsym, func=lambda x: coherence(x, Fc)):
+        if abs(coeff) > COHERENCE_THRESHOLD:
+            counter += 1
+        else:
+            counter = 0
+
+        if counter == CARRIER_THRESHOLD:
+            length = CARRIER_THRESHOLD * Nsym
+            return offset - length + Nsym, offset
+
+def find_start(x, start):
+    WINDOW = Nsym * 10
+    length = CARRIER_DURATION * Nsym
+    begin, end = start - WINDOW, start + length + WINDOW
+    x_ = x[begin:end]
+
+    Hc = exp_iwt(Fc, len(x_))
+    P = np.abs(Hc.conj() * x_) ** 2
+    cumsumP = P.cumsum()
+    start = np.argmax(cumsumP[length:] - cumsumP[:-length]) + begin
+    log.info('Carrier starts at {:.3f} ms'.format(start * Tsym * 1e3 / Nsym))
+    return start
+
+def equalize(symbols):
+    bits = np.round(np.abs(symbols))
+    bits = np.array(bits, dtype=int)
+    prefix = [1]*300 + [0]*100 + ([1]*10 + [0]*10)*20 + [0]*100
+    n = len(prefix)
+    if all(bits[:n] == prefix):
+
+        S = symbols[:n]
+
+        A = np.array([ S[1:], S[:-1], prefix[:-1] ]).T
+        b = prefix[1:]
+
+        b0, b1, a1 = linalg.lstsq(A, b)[0]
+        y = np.array(list(sigproc.lfilter([b0, b1], [1, -a1], symbols)))
+        y = y.conj() # TODO
+        constellation(y)
+
+        prefix_bits, noise = slicer(y[:n], 0.5)
+        assert(all(prefix_bits == np.array(prefix)))
+        log.info( 'Prefix OK, at SNR: {:.1f} dB'.format( 20 * np.log10(np.sqrt(len(prefix_bits)) / norm(noise)) ) )
+
+        data_bits = sigproc.qpsk.decode(y[n:])
+        data_bits = list(data_bits)
+        log.info( 'Demodulated {} payload bits'.format(len(data_bits)) )
+        return data_bits
+
+def slicer(symbols, threshold):
+    bits = symbols.real > threshold
+    noise = symbols - bits
+    return bits, noise
+
+def constellation(y):
+    theta = np.linspace(0, 2*np.pi, 1000)
+
+    pylab.figure()
+    pylab.subplot(121)
+    pylab.plot(y.real, y.imag, '.')
+    pylab.plot(np.cos(theta), np.sin(theta), ':')
+    pylab.grid('on')
+    pylab.axis('equal')
+
+    pylab.subplot(122)
+    pylab.plot(np.arange(len(y)) * Tsym, y.real, '.')
+    pylab.grid('on')
+
+def main(t, x):
+
+    x = (x - np.mean(x))
+    result = detect(Fc)
+    if result is None:
+        log.info('No carrier detected')
+        return
+
+    begin, end = result
+    x_ = x[begin:end]
+    t_ = t[begin:end]
+    Hc = exp_iwt(Fc, len(x_))
+    Zc = np.dot(Hc.conj(), x_) / (0.5*len(x_))
+    amp = abs(Zc)
+    phase = np.angle(Zc, deg=True)
+    log.info('Carrier detected at ~{:.1f} ms @ {:.1f} kHz: coherence={:.3f}%, amplitude={:.3f}'.format(
+          begin * Tsym * 1e3 / Nsym, Fc / 1e3, abs(coherence(x_, Fc)) * 100, amp
+    ))
+
+    start = find_start(x, begin)
+    x = x[start:] / amp
+    t = t[start:]
+
+    Hc = exp_iwt(Fc, Nsym) / (0.5*Nsym)
+    func = lambda y: np.dot(Hc, y)
+    symbols = []
+    for _, coeff in iterate(x, Nsym, advance=Nsym, func=func):
+        symbols.append(coeff)
+    symbols = np.array(symbols)
+
+    data_bits = equalize(symbols)
+    if data_bits is None:
+        log.info('Cannot demodulate symbols!')
+    else:
+        data = iterate(data_bits, bufsize=8, advance=8, func=to_bytes)
+        data = ''.join(c for _, c in data)
+        data = unpack(data)
+        log.info(repr(data))
+
+    pylab.show()
+
+
+if __name__ == '__main__':
+    fname, = sys.argv[1:]
+    t, x = load(fname)
+    main(t, x)
+    pylab.show()
diff --git a/send.py b/send.py
new file mode 100644
index 0000000..e8c3f5d
--- /dev/null
+++ b/send.py
@@ -0,0 +1,72 @@
+from matplotlib import pyplot as plt
+import subprocess as sp
+import numpy as np
+
+import os
+import sys
+import time
+import signal
+import logging
+logging.basicConfig(level=0, format='%(message)s')
+log = logging.getLogger(__name__)
+
+import sigproc
+from common import *
+
+def play(fd):
+    args = ['aplay', fd.name, '-f', 'S16_LE', '-c', '1', '-r', str(int(Fs))]
+    ret = sp.call(args=args)
+    assert ret == 0
+
+def record(fname):
+    args = ['arecord', fname, '-f', 'S16_LE', '-c', '1', '-r', str(int(Fs))]
+    p = sp.Popen(args=args)
+    p.stop = lambda: os.kill(r.pid, signal.SIGINT)
+    return p
+
+
+log.info('MODEM Fc={}KHz, {} BAUD'.format(Fc/1e3, baud))
+
+
+class Symbol(object):
+    t       = np.arange(0, Nsym) * Ts
+#    c0      = np.sin(2 * np.pi * F0 * t)
+#    c1      = np.sin(2 * np.pi * F1 * t)
+    carrier = np.exp(2j * np.pi * Fc * t)
+
+class Signal(object):
+    def __init__(self, fd):
+        self.fd = fd
+    def send(self, sym, n=1):
+        sym = sym.imag * scaling
+        sym = sym.astype('int16')
+        for i in range(n):
+            sym.tofile(fd)
+
+sym = Symbol()
+
+data = os.urandom(1024)
+data = pack(data)
+
+bits = list(to_bits(data))
+
+if __name__ == '__main__':
+
+    with open('tx.int16', 'wb') as fd:
+        sig = Signal(fd)
+        sig.send(sym.carrier*0, n=100)
+        sig.send(sym.carrier*1, n=300)
+        sig.send(sym.carrier*0, n=100)
+        for i in range(20):
+            sig.send(sym.carrier*1, n=10)
+            sig.send(sym.carrier*0, n=10)
+
+        sig.send(sym.carrier*0, n=100)
+        for s in sigproc.qpsk.encode(bits):
+            sig.send(sym.carrier * s)
+
+
+    r = record('rx.int16')
+    p = play(fd)
+    time.sleep(0.2)
+    r.stop()
diff --git a/sigproc.py b/sigproc.py
new file mode 100644
index 0000000..000de11
--- /dev/null
+++ b/sigproc.py
@@ -0,0 +1,53 @@
+import numpy as np
+
+def lfilter(b, a, x):
+    b = np.array(b) / a[0]
+    a = np.array(a[1:]) / a[0]
+
+    x_ = [0] * len(b)
+    y_ = [0] * len(a)
+    for v in x:
+        x_ = [v] + x_[:-1]
+        u  = np.dot(x_, b)
+        u = u - np.dot(y_, a)
+
+        y_ = [u] + y_[1:]
+        yield u
+
+class QPSK(object):
+    def __init__(self, bits_per_symbol):
+        self._enc = {tuple(): 0.0} # End-Of-Stream symbol
+        index = 0
+        amps = [0.6, 1.0]
+        N = (2 ** bits_per_symbol) / len(amps)
+        for a in amps:
+            for i in range(N):
+                k = tuple(int(index & (1 << j) != 0) for j in range(bits_per_symbol))
+                v = np.exp(2j * i * np.pi / N)
+                self._enc[k] = v * a
+                index += 1
+        self._dec = {v: k for k, v in self._enc.items()}
+        self.bits_per_symbol = bits_per_symbol
+
+    def encode(self, bits):
+        assert len(bits) % self.bits_per_symbol == 0
+        for i in range(0, len(bits), self.bits_per_symbol):
+            s = self._enc[ tuple(bits[i:i+self.bits_per_symbol]) ]
+            yield s
+
+    def decode(self, symbols):
+        keys = np.array(self._dec.keys())
+        for s in symbols:
+            index = np.argmin(np.abs(s - keys))
+            bits = self._dec[ keys[index] ]
+            for b in bits:
+                yield b
+
+
+qpsk = QPSK(bits_per_symbol=4)
+
+def test_qpsk():
+    q = QPSK(bits_per_symbol=2)
+    bits = [1,1, 0,1, 0,0, 1,0]
+    S = qpsk.encode(bits)
+    assert list(qpsk.decode(list(S))) == bits