Add main.py and basic functions to get measurement

7 files changed, 407 insertions, 0 deletions

diff --git a/dpd/main.py b/dpd/main.py
new file mode 100644
index 0000000..427bc28
--- /dev/null
+++ b/dpd/main.py
@@ -0,0 +1,41 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import logging
+logging.basicConfig(format='%(asctime)s - %(module)s - %(levelname)s - %(message)s',
+                    datefmt='%Y-%m-%d %H:%M:%S',
+                    filename='/tmp/dpd.log',
+                    filemode='w',
+                    level=logging.DEBUG)
+import src.Measure as Measure
+
+port = 50055
+num_req = 10240
+
+m = Measure.Measure(port, num_req)
+
+logging.info("Do measurement")
+m.get_samples()
+logging.info("Done")
+
+# The MIT License (MIT)
+#
+# Copyright (c) 2017 Andreas Steger
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
diff --git a/dpd/src/Dab_Util.py b/dpd/src/Dab_Util.py
new file mode 100644
index 0000000..73ae852
--- /dev/null
+++ b/dpd/src/Dab_Util.py
@@ -0,0 +1,93 @@
+import numpy as np
+import scipy
+import matplotlib.pyplot as plt
+import src.subsample_align as sa
+from scipy import signal
+import logging
+
+class Dab_Util:
+    """Collection of methods that can be applied to an array
+     complex IQ samples of a DAB signal
+     """
+    def __init__(self, sample_rate):
+        """
+        :param sample_rate: sample rate [sample/sec] to use for calculations
+        """
+        self.sample_rate = sample_rate
+        self.dab_bandwidth = 1536000 #Bandwidth of a dab signal
+        self.frame_ms = 96           #Duration of a Dab frame
+
+    def lag(self, sig_orig, sig_rec):
+        """
+        Find lag between two signals
+        Args:
+            sig_orig: The signal that has been sent
+            sig_rec: The signal that has been recored
+        """
+        off = sig_rec.shape[0]
+        c = signal.correlate(sig_orig, sig_rec)
+        return np.argmax(c) - off + 1
+
+    def lag_upsampling(self, sig_orig, sig_rec, n_up):
+        sig_orig_up = signal.resample(sig_orig, sig_orig.shape[0] * n_up)
+        sig_rec_up  = signal.resample(sig_rec, sig_rec.shape[0] * n_up)
+        l = self.lag(sig_orig_up, sig_rec_up)
+        l_orig = float(l) / n_up
+        return l_orig
+
+    def subsample_align_upsampling(self, sig1, sig2, n_up=32):
+        """
+        Returns an aligned version of sig1 and sig2 by cropping and subsample alignment
+        Using upsampling
+        """
+        assert(sig1.shape[0] == sig2.shape[0])
+
+        if sig1.shape[0] % 2 == 1:
+            sig1 = sig1[:-1]
+            sig2 = sig2[:-1]
+
+        sig1_up = signal.resample(sig1, sig1.shape[0] * n_up)
+        sig2_up = signal.resample(sig2, sig2.shape[0] * n_up)
+
+        off_meas = self.lag_upsampling(sig2_up, sig1_up, n_up=1)
+        off = int(abs(off_meas))
+
+        if off_meas > 0:
+            sig1_up = sig1_up[:-off]
+            sig2_up = sig2_up[off:]
+        elif off_meas < 0:
+            sig1_up = sig1_up[off:]
+        sig2_up = sig2_up[:-off]
+
+        sig1 = signal.resample(sig1_up, sig1_up.shape[0] / n_up).astype(np.complex64)
+        sig2 = signal.resample(sig2_up, sig2_up.shape[0] / n_up).astype(np.complex64)
+        return sig1, sig2
+
+    def subsample_align(self, sig1, sig2):
+        """
+        Returns an aligned version of sig1 and sig2 by cropping and subsample alignment
+        """
+        logging.debug("Sig1_orig: %d %s, Sig2_orig: %d %s" % (len(sig1), sig1.dtype, len(sig2), sig2.dtype))
+        off_meas = self.lag_upsampling(sig2, sig1, n_up=1)
+        off = int(abs(off_meas))
+
+        if off_meas > 0:
+            sig1 = sig1[:-off]
+            sig2 = sig2[off:]
+        elif off_meas < 0:
+            sig1 = sig1[off:]
+            sig2 = sig2[:-off]
+
+        if off % 2 == 1:
+            sig1 = sig1[:-1]
+            sig2 = sig2[:-1]
+
+        sig2 = sa.subsample_align(sig2, sig1)
+        logging.debug("Sig1_cut: %d %s, Sig2_cut: %d %s, off: %d" % (len(sig1), sig1.dtype, len(sig2), sig2.dtype, off))
+        return sig1, sig2
+
+    def fromfile(self, filename, offset=0, length=None):
+        if length is None:
+            return np.memmap(filename, dtype=np.complex64, mode='r', offset=64/8*offset)
+        else:
+            return np.memmap(filename, dtype=np.complex64, mode='r', offset=64/8*offset, shape=length)
diff --git a/dpd/src/Measure.py b/dpd/src/Measure.py
new file mode 100644
index 0000000..a8da137
--- /dev/null
+++ b/dpd/src/Measure.py
@@ -0,0 +1,104 @@
+# The MIT License (MIT)
+#
+# Copyright (c) 2017 Andreas Steger
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+import sys
+import socket
+import struct
+import numpy as np
+import matplotlib.pyplot as pp
+from matplotlib.animation import FuncAnimation
+import argparse
+import os
+import time
+import logging
+import Dab_Util as DU
+
+class Measure:
+    """Collect Measurement from DabMod"""
+    def __init__(self, port, num_samples_to_request):
+        """"""
+        logging.info("Initalized Measure class")
+        self.sizeof_sample = 8 # complex floats
+        self.port = port
+        self.num_samples_to_request = num_samples_to_request
+
+    def _recv_exact(self, sock, num_bytes):
+        """Interfaces the socket to receive a byte string
+        
+        Args:
+            sock (socket): Socket to receive data from.
+            num_bytes (int): Number of bytes that will be returned.
+        """
+        bufs = []
+        while num_bytes > 0:
+            b = sock.recv(num_bytes)
+            if len(b) == 0:
+                break
+            num_bytes -= len(b)
+            bufs.append(b)
+        return b''.join(bufs)
+
+    def get_samples(self):
+        """Connect to ODR-DabMod, retrieve TX and RX samples, load
+        into numpy arrays, and return a tuple
+        (tx_timestamp, tx_samples, rx_timestamp, rx_samples)
+        where the timestamps are doubles, and the samples are numpy
+        arrays of complex floats, both having the same size
+        """
+        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        s.connect(('localhost', self.port))
+
+        logging.debug("Send version");
+        s.sendall(b"\x01")
+
+        logging.debug("Send request for {} samples".format(self.num_samples_to_request))
+        s.sendall(struct.pack("=I", self.num_samples_to_request))
+
+        logging.debug("Wait for TX metadata")
+        num_samps, tx_second, tx_pps = struct.unpack("=III", self._recv_exact(s, 12))
+        tx_ts = tx_second + tx_pps / 16384000.0
+
+        if num_samps > 0:
+            logging.debug("Receiving {} TX samples".format(num_samps))
+            txframe_bytes = self._recv_exact(s, num_samps * self.sizeof_sample)
+            txframe = np.fromstring(txframe_bytes, dtype=np.complex64)
+        else:
+            txframe = np.array([], dtype=np.complex64)
+
+        logging.debug("Wait for RX metadata")
+        rx_second, rx_pps = struct.unpack("=II", self._recv_exact(s, 8))
+        rx_ts = rx_second + rx_pps / 16384000.0
+
+        if num_samps > 0:
+            logging.debug("Receiving {} RX samples".format(num_samps))
+            rxframe_bytes = self._recv_exact(s, num_samps * self.sizeof_sample)
+            rxframe = np.fromstring(rxframe_bytes, dtype=np.complex64)
+        else:
+            rxframe = np.array([], dtype=np.complex64)
+
+        logging.debug("Disconnecting")
+        s.close()
+
+        logging.info("Measurement done, txframe %d %s, rxframe %d %s" % (len(txframe), txframe.dtype, len(rxframe), rxframe.dtype) )
+        du = DU.Dab_Util(8192000)
+        txframe_aligned, rxframe_aligned = du.subsample_align(txframe, rxframe)
+        return txframe_aligned, rxframe_aligned
diff --git a/dpd/src/__init__.py b/dpd/src/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/dpd/src/__init__.py
diff --git a/dpd/src/subsample_align.py b/dpd/src/subsample_align.py
new file mode 100755
index 0000000..c30af7c
--- /dev/null
+++ b/dpd/src/subsample_align.py
@@ -0,0 +1,74 @@
+import numpy as np
+from scipy import signal, optimize
+import sys
+import matplotlib.pyplot as plt
+
+def gen_omega(length):
+    if (length % 2) == 1:
+        raise ValueError("Needs an even length array.")
+
+    halflength = int(length/2)
+    factor = 2.0 * np.pi / length
+
+    omega = np.zeros(length, dtype=np.float)
+    for i in range(halflength):
+        omega[i] = factor * i
+
+    for i in range(halflength, length):
+        omega[i] = factor * (i - length)
+
+    return omega
+
+def subsample_align(sig, ref_sig):
+    """Do subsample alignment for sig relative to the reference signal
+    ref_sig. The delay between the two must be less than sample
+
+    Returns the aligned signal"""
+
+    n = len(sig)
+    if (n % 2) == 1:
+        raise ValueError("Needs an even length signal.")
+    halflen = int(n/2)
+
+    fft_sig = np.fft.fft(sig)
+
+    omega = gen_omega(n)
+
+    def correlate_for_delay(tau):
+        # A subsample offset between two signals corresponds, in the frequency
+        # domain, to a linearly increasing phase shift, whose slope
+        # corresponds to the delay.
+        #
+        # Here, we build this phase shift in rotate_vec, and multiply it with
+        # our signal.
+
+        rotate_vec = np.exp(1j * tau * omega)
+        # zero-frequency is rotate_vec[0], so rotate_vec[N/2] is the
+        # bin corresponding to the [-1, 1, -1, 1, ...] time signal, which
+        # is both the maximum positive and negative frequency.
+        # I don't remember why we handle it differently.
+        rotate_vec[halflen] = np.cos(np.pi * tau)
+
+        corr_sig = np.fft.ifft(rotate_vec * fft_sig)
+
+        # TODO why do we only look at the real part? Because it's faster than
+        # a complex cross-correlation? Clarify!
+        return -np.sum(np.real(corr_sig) * np.real(ref_sig.real))
+
+    optim_result = optimize.minimize_scalar(correlate_for_delay, bounds=(-1,1), method='bounded', options={'disp': True})
+
+    if optim_result.success:
+        #print("x:")
+        #print(optim_result.x)
+
+        best_tau = optim_result.x
+
+        #print("Found subsample delay = {}".format(best_tau))
+
+        # Prepare rotate_vec = fft_sig with rotated phase
+        rotate_vec = np.exp(1j * best_tau * omega)
+        rotate_vec[halflen] = np.cos(np.pi * best_tau)
+        return np.fft.ifft(rotate_vec * fft_sig).astype(np.complex64)
+    else:
+        #print("Could not optimize: " + optim_result.message)
+        return np.zeros(0, dtype=np.complex64)
diff --git a/dpd/src/test_dab_Util.py b/dpd/src/test_dab_Util.py
new file mode 100644
index 0000000..0b2fa4f
--- /dev/null
+++ b/dpd/src/test_dab_Util.py
@@ -0,0 +1,62 @@
+from unittest import TestCase
+
+import numpy as np
+import pandas as pd
+import src.Dab_Util as DU
+
+class TestDab_Util(TestCase):
+
+    def test_subsample_align(self, sample_orig=r'../test_data/orig_rough_aligned.dat',
+                             sample_rec =r'../test_data/recored_rough_aligned.dat',
+                             length = 10240, max_size = 1000000):
+        du = DU.Dab_Util(8196000)
+        res1 = []
+        res2 = []
+        for i in range(10):
+            start = np.random.randint(50, max_size)
+            r = np.random.randint(-50, 50)
+
+            s1 = du.fromfile(sample_orig, offset=start+r, length=length)
+            s2 = du.fromfile(sample_rec, offset=start, length=length)
+
+            res1.append(du.lag_upsampling(s2, s1, 32))
+
+            s1_aligned, s2_aligned = du.subsample_align(s1, s2)
+
+            res2.append(du.lag_upsampling(s2_aligned, s1_aligned, 32))
+
+        error_rate = np.mean(np.array(res2) != 0)
+        self.assertEqual(error_rate, 0.0, "The error rate for aligning was %.2f%%"
+                         % error_rate * 100)
+
+#def test_using_aligned_pair(sample_orig=r'../data/orig_rough_aligned.dat', sample_rec =r'../data/recored_rough_aligned.dat', length = 10240, max_size = 1000000):
+#    res = []
+#    for i in tqdm(range(100)):
+#        start = np.random.randint(50, max_size)
+#        r = np.random.randint(-50, 50)
+#
+#        s1 = du.fromfile(sample_orig, offset=start+r, length=length)
+#        s2 = du.fromfile(sample_rec, offset=start, length=length)
+#
+#        res.append({'offset':r,
+#                    '1':r - du.lag_upsampling(s2, s1, n_up=1),
+#                    '2':r - du.lag_upsampling(s2, s1, n_up=2),
+#                    '3':r - du.lag_upsampling(s2, s1, n_up=3),
+#                    '4':r - du.lag_upsampling(s2, s1, n_up=4),
+#                    '8':r - du.lag_upsampling(s2, s1, n_up=8),
+#                    '16':r - du.lag_upsampling(s2, s1, n_up=16),
+#                    '32':r - du.lag_upsampling(s2, s1, n_up=32),
+#                    })
+#    df = pd.DataFrame(res)
+#    df = df.reindex_axis(sorted(df.columns), axis=1)
+#    print(df.describe())
+#
+#
+#print("Align using upsampling")
+#for n_up in [1, 2, 3, 4, 7, 8, 16]:
+#   correct_ratio = test_phase_offset(lambda x,y: du.lag_upsampling(x,y,n_up), tol=1./n_up)
+#   print("%.1f%% of the tested offsets were measured within tolerance %.4f for n_up = %d" % (correct_ratio * 100, 1./n_up, n_up))
+#test_using_aligned_pair()
+#
+#print("Phase alignment")
+#test_subsample_alignment()
diff --git a/dpd/src/test_measure.py b/dpd/src/test_measure.py
new file mode 100644
index 0000000..b695721
--- /dev/null
+++ b/dpd/src/test_measure.py
@@ -0,0 +1,33 @@
+from unittest import TestCase
+from Measure import Measure
+import socket
+
+
+class TestMeasure(TestCase):
+
+    def _open_socks(self):
+        sock_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        sock_server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        sock_server.bind(('localhost', 1234))
+        sock_server.listen(1)
+
+        sock_client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        sock_client.connect(('localhost', 1234))
+
+        conn_server, addr_server = sock_server.accept()
+        return conn_server, sock_client
+
+    def test__recv_exact(self):
+        m = Measure(1234, 1)
+        payload = b"test payload"
+
+        conn_server, sock_client = self._open_socks()
+        conn_server.send(payload)
+        rec = m._recv_exact(sock_client, len(payload))
+
+        self.assertEqual(rec, payload,
+                "Did not receive the same message as sended. (%s, %s)" %
+                (rec, payload))
+
+    def test_get_samples(self):
+        self.fail()