From cb1f1c047eb8eae2042d350c5cdba18b762dc1da Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Tue, 11 Aug 2020 17:48:11 +0800
Subject: [PATCH] plot position

---
 dmi.py      | 24 +++++++++++++-----------
 gui.py      |  4 ++++
 gui_impl.py | 11 ++++++++++-
 noptica.py  | 14 +++-----------
 4 files changed, 30 insertions(+), 23 deletions(-)

diff --git a/dmi.py b/dmi.py
index 5759c44..bc72266 100644
--- a/dmi.py
+++ b/dmi.py
@@ -24,34 +24,36 @@ def main():
                 induction.set(tuning)
 
             stabilizer = Stabilizer(freq_sample, block_size, 1088.1e6 - freq_base, stabilizer_cb)
-            position_tracker = PositionTracker(int(0.1*freq_sample/block_size))
+            position_tracker = PositionTracker()
 
             sdr = SoapySDR.Device()
             for channel in range(2):
                 sdr.setSampleRate(SoapySDR.SOAPY_SDR_RX, channel, freq_sample)
                 sdr.setFrequency(SoapySDR.SOAPY_SDR_RX, channel, freq_base)
 
-            buf_sdr = BufferedSDR(sdr, [0, 1], block_size, 256)
+            buf_sdr = BufferedSDR(sdr, [0, 1], block_size, 32)
             buf_sdr.start()
             try:
-                stabilizer_throttle = 0
+                throttle = 0
                 while True:
                     buffers = buf_sdr.get()
                     try:
                         samples_ref, samples_meas = buffers
 
-                        # We can't update faster than the MHS5200A serial interface
-                        stabilizer_throttle += 1
-                        if stabilizer_throttle == 8:
-                            stabilizer.input(samples_ref)
-                            stabilizer_throttle = 0
+                        # Throttle certain things to avoid overflows due to the limited speed of
+                        # the MHS5200A serial interface and GUI plotting.
+                        throttle += 1
+                        if throttle == 8:
+                            throttle = 0
 
-                            # Update the MEAS GUI at the same time so it's synchronized
+                        if throttle == 0:
+                            stabilizer.input(samples_ref)
                             gui.update_meas(samples_meas)
 
                         if stabilizer.locked():
-                            position, leakage = position_tracker.input(samples_ref, samples_meas)
-                            print(np.sum(position)/len(position), leakage)
+                            position = position_tracker.input(samples_ref, samples_meas)
+                            if throttle == 0:
+                                gui.update_position(np.sum(position)/len(position))
                         else:
                             position_tracker.reset()
                     finally:
diff --git a/gui.py b/gui.py
index 7b5d2a2..5c5d5f9 100644
--- a/gui.py
+++ b/gui.py
@@ -62,6 +62,10 @@ class GUI:
         obj = {"action": "update_meas", "block": block}
         self.impl.write_pyon(obj)
 
+    def update_position(self, position):
+        obj = {"action": "update_position", "position": position}
+        self.impl.write_pyon(obj)
+
     def close(self):
         obj = {"action": "terminate"}
         self.impl.write_pyon(obj)
diff --git a/gui_impl.py b/gui_impl.py
index 444361d..a4d1dd8 100644
--- a/gui_impl.py
+++ b/gui_impl.py
@@ -23,7 +23,7 @@ class SpectrogramItem(pg.ImageItem):
 
     def add_block(self, block):
         self.img_array = np.roll(self.img_array, -1, 0)
-        self.img_array[-1:] = np.fft.fftshift(block) 
+        self.img_array[-1:] = np.fft.fftshift(block)
         self.setImage(self.img_array, autoLevels=True, autoDownsample=True)
 
 
@@ -50,6 +50,9 @@ class MainWindow(pg.GraphicsLayoutWidget):
         self.meas_spectrum = SpectrogramItem(freq_sample, freq_base, block_size)
         p2.addItem(self.meas_spectrum)
 
+        self.position = self.addPlot(row=3, col=0, colspan=2)
+        self.position_history = np.zeros(300)
+
     def update_ref(self, block, peak_freq, locked):
         if block is not None:
             self.ref_spectrum.add_block(block)
@@ -67,6 +70,12 @@ class MainWindow(pg.GraphicsLayoutWidget):
         spectrum = np.abs(np.fft.fft(block*blackmanharris(self.block_size)))
         self.meas_spectrum.add_block(spectrum)
 
+    def update_position(self, position):
+        self.position_history = np.roll(self.position_history, -1)
+        self.position_history[-1] = position
+        self.position.clear()
+        self.position.plot(self.position_history)
+
 
 class IPCClient(AsyncioChildComm):
     def set_close_cb(self, close_cb):
diff --git a/noptica.py b/noptica.py
index df8c43f..75ed5e8 100644
--- a/noptica.py
+++ b/noptica.py
@@ -169,25 +169,17 @@ def continuous_unwrap(last_phase, last_phase_unwrapped, p):
 
 
 class PositionTracker:
-    def __init__(self, leakage_avg):
-        self.leakage_avg = leakage_avg
+    def __init__(self):
         self.reset()
 
     def reset(self):
         self.last_phase = 0.0
         self.last_position = 0.0
-        self.leakage = np.zeros(self.leakage_avg)
-        self.leakage_ptr = 0
 
     def input(self, ref, meas):
         demod = np.conjugate(ref)*meas
-
-        self.leakage[self.leakage_ptr] = np.real(np.sum(demod)/len(demod))
-        self.leakage_ptr = (self.leakage_ptr + 1) % len(self.leakage)
-        leakage = np.sum(self.leakage)/len(self.leakage)
-
-        phase = np.angle(demod - leakage)
+        phase = np.angle(demod)
         position = continuous_unwrap(self.last_phase, self.last_position, phase)/(2.0*np.pi)
         self.last_phase = phase[-1]
         self.last_position = position[-1]
-        return position, leakage
+        return position