From ce2cdd10b6cb4b458b469811b58c6b2bd522c8e3 Mon Sep 17 00:00:00 2001 From: Sebastien Bourdeauducq 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("", 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)