complete DMI code, untested

dmi.py

@@ -1,40 +1,46 @@
 import SoapySDR
 import numpy as np
-from scipy.signal import blackmanharris
 
-from stabilizer import InductionHeater, Stabilizer
+from noptica import InductionHeater, Stabilizer, PositionTracker
 
 
 def main():
     freq_sample = 5e6
     freq_base = 1086e6
+    bufsize = 4096
 
     induction = InductionHeater("/dev/ttyUSB0", 430e3, 445e3)
     induction.start()
     try:
         stabilizer = Stabilizer(freq_sample, 0.4, 1088.3e6 - freq_base, 200e-6, induction)
+        position_tracker = PositionTracker(int(0.1*freq_sample/bufsize))
 
         sdr = SoapySDR.Device()
-        sdr.setSampleRate(SoapySDR.SOAPY_SDR_RX, 0, freq_sample)
+        for channel in range(2):
-        sdr.setFrequency(SoapySDR.SOAPY_SDR_RX, 0, base_freq)
+            sdr.setSampleRate(SoapySDR.SOAPY_SDR_RX, channel, freq_sample)
+            sdr.setFrequency(SoapySDR.SOAPY_SDR_RX, channel, freq_base)
 
-        samples = np.array([0]*4096, np.complex64)
+        samples_ref = np.array([0]*bufsize, np.complex64)
-        rx_stream = sdr.setupStream(SoapySDR.SOAPY_SDR_RX, SoapySDR.SOAPY_SDR_CF32)
+        samples_meas = np.array([0]*bufsize, np.complex64)
+        rx_stream = sdr.setupStream(SoapySDR.SOAPY_SDR_RX, SoapySDR.SOAPY_SDR_CF32, [0, 1])
         try:
             sdr.activateStream(rx_stream)
             try:
-                # We can't update faster than the MHS5200A serial interface
                 stabilizer_throttle = 0
                 while True:
-                    sr = sdr.readStream(rx_stream, [samples], len(samples))
+                    sr = sdr.readStream(rx_stream, [samples_ref, samples_meas], bufsize)
-                    if sr.ret != len(samples):
+                    if sr.ret != bufsize:
                         print("SDR sampling error")
                         return
 
+                    # We can't update faster than the MHS5200A serial interface
                     stabilizer_throttle += 1
                     if stabilizer_throttle == 30:
-                        stabilizer.input(samples)
+                        stabilizer.input(samples_ref)
                         stabilizer_throttle = 0
+
+                    position, leakage = position_tracker.input(samples_ref, samples_meas)
+                    print(np.sum(position)/len(position), leakage)
             finally:
                 sdr.deactivateStream(rx_stream)
         finally:

noptica.py

@@ -1,6 +1,7 @@
 import serial
 import queue
 import threading
+
 import numpy as np
 from scipy.signal import blackmanharris
 
@@ -70,3 +71,32 @@ class Stabilizer:
         else:
             tuning = 0.0
         self.tuner.set(tuning)
+
+
+def continuous_unwrap(last_phase, last_phase_unwrapped, p):
+    # note: np.unwrap always preserves first element of array
+    p = np.unwrap(p)
+    glue = np.array([last_phase_unwrapped, last_phase_unwrapped + (p[0] - last_phase)])
+    new_p0 = np.unwrap(glue)[1]
+    return new_p0 + p - p[0]
+
+
+class PositionTracker:
+    def __init__(self, leakage_avg):
+        self.last_phase = 0.0
+        self.last_position = 0.0
+        self.leakage = np.zeros(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)
+        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