From ce2cdd10b6cb4b458b469811b58c6b2bd522c8e3 Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Sat, 11 Jan 2020 16:06:54 +0800
Subject: [PATCH] refactor

---
 stabilize.py  | 80 ++++++++++++++++-----------------------------------
 stabilizer.py | 33 +++++++++++++++++++++
 2 files changed, 58 insertions(+), 55 deletions(-)

diff --git a/stabilize.py b/stabilize.py
index a40eef6..346c17c 100644
--- a/stabilize.py
+++ b/stabilize.py
@@ -2,73 +2,43 @@ import SoapySDR
 import numpy as np
 from scipy.signal import blackmanharris
 
-from stabilizer import InductionHeater
-
-fs = 5e6
-base_freq = 1086e6
-threshold = 0.4
-target_freq = 1088.3e6
-k = 200e-6
-
-def parabolic(f, x):
-    xv = 1/2. * (f[x-1] - f[x+1]) / (f[x-1] - 2 * f[x] + f[x+1]) + x
-    yv = f[x] - 1/4. * (f[x-1] - f[x+1]) * (xv - x)
-    return (xv, yv)
-
-
-def fmt_range(x, m):
-    pos = int(round(x*80/m))
-    return " "*pos + "*"
+from stabilizer import InductionHeater, Stabilizer
 
 
 def main():
+    freq_sample = 5e6
+    freq_base = 1086e6
+
     induction = InductionHeater("/dev/ttyUSB0", 430e3, 445e3)
     induction.start()
     try:
-        sdr = SoapySDR.Device()
-        samples = np.array([0]*4096, np.complex64)
+        stabilizer = Stabilizer(freq_sample, 0.4, 1088.3e6 - freq_base, 200e-6, induction)
 
-        sdr.setSampleRate(SoapySDR.SOAPY_SDR_RX, 0, fs)
+        sdr = SoapySDR.Device()
+        sdr.setSampleRate(SoapySDR.SOAPY_SDR_RX, 0, freq_sample)
         sdr.setFrequency(SoapySDR.SOAPY_SDR_RX, 0, base_freq)
 
-        rxStream = sdr.setupStream(SoapySDR.SOAPY_SDR_RX, SoapySDR.SOAPY_SDR_CF32)
-        sdr.activateStream(rxStream)
-
-        decimation = 0
-
+        samples = np.array([0]*4096, np.complex64)
+        rx_stream = sdr.setupStream(SoapySDR.SOAPY_SDR_RX, SoapySDR.SOAPY_SDR_CF32)
         try:
-            while True:
-                sr = sdr.readStream(rxStream, [samples], len(samples))
-                if sr.ret != len(samples):
-                    print("!")
-                    continue
+            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))
+                    if sr.ret != len(samples):
+                        print("SDR sampling error")
+                        return
 
-                freqs = np.abs(np.fft.fft(samples*blackmanharris(len(samples)))[0:len(samples)//2])
-                for i in range(len(freqs)//100):
-                    freqs[i] = 0
-                    freqs[-i] = 0
-
-                # https://gist.github.com/endolith/255291
-                i = np.argmax(freqs)
-                true_i = parabolic(freqs, i)[0]
-                freq = 0.5 * fs * true_i / len(freqs)
-                amplitude = freqs[i]
-
-                if amplitude > threshold:
-                    actual_freq = base_freq + freq
-                    print("{:8.3f} {:6.2f} {}".format(actual_freq/1e6, amplitude, fmt_range(freq, fs/2)))
-                    induction_amt = (actual_freq - target_freq)*k
-                else:
-                    print("<no signal>", amplitude)
-                    induction_amt = 0.0
-
-                decimation += 1
-                if decimation == 30:
-                    induction.set(induction_amt)
-                    decimation = 0
+                    stabilizer_throttle += 1
+                    if stabilizer_throttle == 30:
+                        stabilizer.input(samples)
+                        stabilizer_throttle = 0
+            finally:
+                sdr.deactivateStream(rx_stream)
         finally:
-            sdr.deactivateStream(rxStream)
-            sdr.closeStream(rxStream)
+            sdr.closeStream(rx_stream)
     finally:
         induction.stop()
 
diff --git a/stabilizer.py b/stabilizer.py
index 7fe2504..58967d5 100644
--- a/stabilizer.py
+++ b/stabilizer.py
@@ -1,6 +1,8 @@
 import serial
 import queue
 import threading
+import numpy as np
+from scipy.signal import blackmanharris
 
 
 class InductionHeater:
@@ -37,3 +39,34 @@ class InductionHeater:
         self.queue.put(None, block=True)
         self.thread.join()
         self.serial.close()
+
+
+# https://gist.github.com/endolith/255291
+def parabolic(f, x):
+    xv = 1/2. * (f[x-1] - f[x+1]) / (f[x-1] - 2 * f[x] + f[x+1]) + x
+    yv = f[x] - 1/4. * (f[x-1] - f[x+1]) * (xv - x)
+    return (xv, yv)
+
+
+class Stabilizer:
+    def __init__(self, freq_sample, amp_threshold, freq_target, k, tuner):
+        self.freq_sample = freq_sample
+        self.amp_threshold = amp_threshold
+        self.freq_target = freq_target
+        self.k = k
+        self.tuner = tuner
+
+    def input(self, samples):
+        spectrum = np.abs(np.fft.fft(samples*blackmanharris(len(samples)))[0:len(samples)//2])
+        for i in range(len(spectrum)//100):
+            spectrum[i] = 0
+            spectrum[-i] = 0
+        i = np.argmax(spectrum)
+        true_i, amplitude = parabolic(spectrum, i)
+        freq = 0.5 * self.freq_sample * true_i / len(spectrum)
+
+        if amplitude > threshold:
+            tuning = (freq - target_freq)*k
+        else:
+            tuning = 0.0
+        self.tuner.set(tuning)