2 files changed, 60 insertions, 0 deletions
diff --git a/dpd/src/Dab_Util.py b/dpd/src/Dab_Util.py
index e85d45d..a4a271e 100644
--- a/dpd/src/Dab_Util.py
+++ b/dpd/src/Dab_Util.py
@@ -7,6 +7,7 @@ matplotlib.use('agg')
 import matplotlib.pyplot as plt
 import datetime
 import src.subsample_align as sa
+import src.phase_align as pa
 from scipy import signal
 import logging
 
@@ -123,6 +124,17 @@ class Dab_Util:
             rxframe_path = ('/tmp/rx_2_' + dt + '.iq')
             sig2.tofile(rxframe_path)
 
+        sig2 = pa.phase_align(sig2, sig1)
+
+        sig2 = sa.subsample_align(sig2, sig1)
+        sig2 = pa.phase_align(sig2, sig1)
+
+        if logging.getLogger().getEffectiveLevel() == logging.DEBUG:
+            txframe_path = ('/tmp/tx_3_' + dt + '.iq')
+            sig1.tofile(txframe_path)
+            rxframe_path = ('/tmp/rx_3_' + dt + '.iq')
+            sig2.tofile(rxframe_path)
+
         logging.debug("Sig1_cut: %d %s, Sig2_cut: %d %s, off: %d" % (len(sig1), sig1.dtype, len(sig2), sig2.dtype, off))
         return sig1, sig2
 
diff --git a/dpd/src/phase_align.py b/dpd/src/phase_align.py
new file mode 100644
index 0000000..9e78483
--- /dev/null
+++ b/dpd/src/phase_align.py
@@ -0,0 +1,48 @@
+import numpy as np
+from scipy import signal, optimize
+import sys
+import matplotlib.pyplot as plt
+import datetime
+import logging
+
+
+def phase_align(sig, ref_sig):
+    """Do phase alignment for sig relative to the reference signal
+    ref_sig.
+
+    Returns the aligned signal"""
+
+    angle_diff = (np.angle(sig) - np.angle(ref_sig)) % (2. * np.pi)
+
+    dt = datetime.datetime.now().isoformat()
+    fig_path = "/tmp/phase_align_" + dt + ".pdf"
+    plt.subplot(311)
+    plt.hist(angle_diff, bins=60, label="Angle Diff")
+    plt.xlabel("Angle")
+    plt.ylabel("Count")
+    plt.legend(loc=4)
+
+    plt.subplot(312)
+    plt.plot(np.angle(ref_sig[:128]), label="ref_sig")
+    plt.plot(np.angle(sig[:128]), label="sig")
+    plt.xlabel("Angle")
+    plt.ylabel("Sample")
+    plt.legend(loc=4)
+
+    angle = np.median(angle_diff)
+    logging.debug("Compensating phase by {} rad, {} degree.".format(
+        angle, angle*180./np.pi
+    ))
+    sig = sig * np.exp(1j * -angle)
+
+    plt.subplot(313)
+    plt.plot(np.angle(ref_sig[:128]), label="ref_sig")
+    plt.plot(np.angle(sig[:128]), label="sig")
+    plt.xlabel("Angle")
+    plt.ylabel("Sample")
+    plt.legend(loc=4)
+    plt.tight_layout()
+    plt.savefig(fig_path)
+    plt.clf()
+
+    return sig