You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

166 lines
4.9KB

  1. import serial
  2. import queue
  3. import threading
  4. import SoapySDR
  5. import numpy as np
  6. from scipy.signal import blackmanharris
  7. class BufferedSDR:
  8. def __init__(self, sdr, channels, bufsize, nbufs):
  9. self.sdr = sdr
  10. self.channels = channels
  11. self.bufsize = bufsize
  12. self.stream = None
  13. self.terminate = False
  14. self.thread = None
  15. self.queue = queue.Queue(nbufs)
  16. self.available_buffers = queue.Queue(nbufs)
  17. for _ in range(nbufs):
  18. self.available_buffers.put([np.array([0]*bufsize, np.complex64) for _ in self.channels])
  19. def start(self):
  20. self.stream = self.sdr.setupStream(SoapySDR.SOAPY_SDR_RX, SoapySDR.SOAPY_SDR_CF32, self.channels)
  21. try:
  22. self.thread = threading.Thread(target=self.thread_target)
  23. self.thread.start()
  24. except:
  25. self.sdr.closeStream(self.stream)
  26. raise
  27. def stop(self):
  28. self.terminate = True
  29. self.thread.join()
  30. self.sdr.closeStream(self.stream)
  31. def get(self):
  32. return self.queue.get()
  33. def dispose(self, buffers):
  34. self.available_buffers.put(buffers)
  35. def thread_target(self):
  36. self.sdr.activateStream(self.stream)
  37. try:
  38. while not self.terminate:
  39. buffers = self.available_buffers.get()
  40. sr = self.sdr.readStream(self.stream, buffers, self.bufsize)
  41. if sr.ret != self.bufsize:
  42. print("SDR sampling error")
  43. return
  44. self.queue.put(buffers)
  45. finally:
  46. self.sdr.deactivateStream(self.stream)
  47. class DummyInductionHeater:
  48. def __init__(self, port, induction_min, induction_max):
  49. pass
  50. def start(self):
  51. pass
  52. def set(self, amount):
  53. print("induction", amount)
  54. def stop(self):
  55. pass
  56. class InductionHeater:
  57. """Interface to the MHS5200A function generator driving the LC tank"""
  58. def __init__(self, port, induction_min, induction_max):
  59. self.port = port
  60. self.induction_min = induction_min
  61. self.induction_max = induction_max
  62. self.queue = queue.Queue(1)
  63. def start(self):
  64. self.serial = serial.Serial(self.port, 57600)
  65. self.thread = threading.Thread(target=self.thread_target)
  66. self.thread.start()
  67. def thread_target(self):
  68. while True:
  69. amount = self.queue.get()
  70. if amount is None:
  71. break
  72. amount = max(min(amount, 0.5), -0.5)
  73. freq = ((self.induction_min + self.induction_max)/2
  74. + amount*(self.induction_max - self.induction_min))
  75. command = ":s1f{:010d}\n".format(int(freq*1e2))
  76. self.serial.write(command.encode())
  77. self.serial.readline()
  78. def set(self, amount):
  79. self.queue.put(amount, block=False)
  80. def stop(self):
  81. self.queue.put(None, block=True)
  82. self.thread.join()
  83. self.serial.close()
  84. # https://gist.github.com/endolith/255291
  85. def parabolic(f, x):
  86. xv = 1/2. * (f[x-1] - f[x+1]) / (f[x-1] - 2 * f[x] + f[x+1]) + x
  87. yv = f[x] - 1/4. * (f[x-1] - f[x+1]) * (xv - x)
  88. return (xv, yv)
  89. class Stabilizer:
  90. def __init__(self, freq_sample, amp_threshold, freq_target, k, tuner):
  91. self.freq_sample = freq_sample
  92. self.amp_threshold = amp_threshold
  93. self.freq_target = freq_target
  94. self.k = k
  95. self.tuner = tuner
  96. def input(self, samples):
  97. spectrum = np.abs(np.fft.fft(samples*blackmanharris(len(samples)))[0:len(samples)//2])
  98. for i in range(len(spectrum)//100):
  99. spectrum[i] = 0
  100. spectrum[-i] = 0
  101. i = np.argmax(spectrum)
  102. true_i, amplitude = parabolic(spectrum, i)
  103. freq = 0.5 * self.freq_sample * true_i / len(spectrum)
  104. if amplitude > self.amp_threshold:
  105. tuning = (freq - self.freq_target)*self.k
  106. else:
  107. tuning = 0.0
  108. self.tuner.set(tuning)
  109. def continuous_unwrap(last_phase, last_phase_unwrapped, p):
  110. # note: np.unwrap always preserves first element of array
  111. p = np.unwrap(p)
  112. glue = np.array([last_phase_unwrapped, last_phase_unwrapped + (p[0] - last_phase)])
  113. new_p0 = np.unwrap(glue)[1]
  114. return new_p0 + p - p[0]
  115. class PositionTracker:
  116. def __init__(self, leakage_avg):
  117. self.last_phase = 0.0
  118. self.last_position = 0.0
  119. self.leakage = np.zeros(leakage_avg)
  120. self.leakage_ptr = 0
  121. def input(self, ref, meas):
  122. demod = np.conjugate(ref)*meas
  123. self.leakage[self.leakage_ptr] = np.real(np.sum(demod)/len(demod))
  124. self.leakage_ptr = (self.leakage_ptr + 1) % len(self.leakage)
  125. leakage = np.sum(self.leakage)/len(self.leakage)
  126. phase = np.angle(demod - leakage)
  127. position = continuous_unwrap(self.last_phase, self.last_position, phase)/(2.0*np.pi)
  128. self.last_phase = phase[-1]
  129. self.last_position = position[-1]
  130. return position, leakage